#include <iostream>
#include <fstream>
#include <sstream>
#include <iomanip>
#include <algorithm>
#include <vector>
#define _USE_MATH_DEFINES
#include <cmath>
#include <ctime>

#include "highgui.h"
#include "cv.h"
#include "SharedMattingHeader.h"
#include "UsingSVM.h"

using namespace std;
using namespace cv;

#define sqr(testVec) ((testVec)*(testVec))

const int SPLIT = 1;

typedef struct CvPointList {
    int length;
    CvPoint* data;
} CvPointList;

CvMat *img, *imgMask, *imgMaskGray, *gt;
CvPointList* unknownList;
int** un2PosMap;
int pn;
int unknownPixels;
bool verbose = true;

CvPointList unsampled;
CvPointList fgPoint[sqr(SPLIT+1)], bgPoint[sqr(SPLIT+1)];

//only support 3 channel
typedef struct CvScalarI {
    const uchar* val;
    int data[3];

	CvScalarI operator + (const CvScalarI& src) const {
		CvScalarI res;
		for (int i=0; i<3; ++i)
			res.data[i] = this->data[i] + src.data[i];
		res.val = NULL;
		return res;
	}
	
    CvScalarI operator - (const CvScalarI& src2) const {
        CvScalarI res;
        for (int i=0; i<3; ++i)
            res.data[i] = this->data[i] - src2.data[i];
        res.val = NULL;
        return res;
    }

    CvScalar operator - (const CvScalar& src2) const {
        CvScalar res = cvScalar(0);
        for (int i=0; i<3; ++i)
            res.val[i] = this->data[i] - src2.val[i];
        return res;
    }

    CvScalarI& operator ++() {//prefix
        if (val == NULL)
            for (int i=0; i<3; ++i)
                data[i] = -1;
        else {
            val += 3;
            for (int i=0; i<3; ++i)
                data[i] = *(val+i);
        }
        return *this;
    }

    const CvScalarI operator ++(int) {//postfix
        ++(*this);
        return *this;
    }

    bool operator == (const CvScalarI& src) const {
        for (int i=0; i<3; ++i)
            if (this->data[i] != src.data[i]) return false;
        return true;
    }

    CvScalarI operator * (int sca) const {
        CvScalarI res;
        for (int i=0; i<3; ++i)
            res.data[i] = sca * this->data[i];
        res.val = NULL;
        return res;
    }

    CvScalar operator * (double sca) const {
        CvScalar res = cvScalar(0);
        for (int i=0; i<3; ++i)
            res.val[i] = sca * this->data[i];
        return res;
    }

	CvScalarI operator / (int sca) const {
		CvScalarI res;
		for (int i=0; i<3; ++i)
			res.data[i] = this->data[i] / sca;
		res.val = NULL;
		return res;
	}

} CvScalarI;

//------------------------------------------------------------------------------------------------

void showRGB(CvScalar sca) {
	cout << sca.val[2] << " " << sca.val[1] << " " << sca.val[0] << endl;
}

void printMat(CvMat* mat, ostream* out = &cout, bool bgr = false) {
    int cn = CV_MAT_CN(mat->type);
    for (int i=0; i<mat->rows; ++i) {
        for (int j=0; j<mat->cols; ++j) {
            CvScalar sca = cvGet2D(mat, i, j);
            if (cn == 3) {
				if (bgr)
	                (*out) << "\t(" << sca.val[2] << ", " << sca.val[1] << ", " << sca.val[0] << ")";
				else
					(*out) << "\t(" << sca.val[0] << ", " << sca.val[1] << ", " << sca.val[2] << ")";
            }
            else {
                (*out) << "\t" << sca.val[0];
            }
                
        }
        (*out) << endl;
    }
}

void printMat(CvMat* mat, char filename[], bool bgr = false) {
    ofstream fout;
    fout.open(filename);
    printMat(mat, &fout, bgr);
    fout.close();
}

CvMat *loadMat(char filename[], int cn = 1, bool isUchar = false) {
	static char line[100000];
	CvMat *ret;
	FILE *inFile;
	fopen_s(&inFile, filename, "r");
	if (isUchar) {
		vector<uchar> data;
		uchar val;
		int rows = 0, cols = 0;
		while (fgets(line, 100000, inFile) != NULL) {
			istringstream ins(line);
			if (cn == 1) {
				while (ins >> val) {
					if (rows == 0) ++cols;
					data.push_back(val);
				}
			}
			else {
				cout << "didn't handle. loadMat failed." << endl;
				cols /= cn;
			}
			++rows;
		}
		ret = cvCreateMat(rows, cols, CV_MAKE_TYPE(CV_8U, cn));
		uchar *ptr = ret->data.ptr;
		for (vector<uchar>::iterator it=data.begin(); it!=data.end(); ++it)
			*ptr = *it;
	}
	else {
		vector<float> data;
		float val;
		int rows = 0, cols = 0;
		while (fgets(line, 100000, inFile) != NULL) {
			istringstream ins(line);
			int cnt = 0;
			if (cn == 1) {
				while (ins >> val) {
					++cnt;
//					ins >> val;
//					if (ins.flags() == ios::failbit);
					if (rows == 0) ++cols;
					data.push_back(val);
				}
				if (cnt < 800) {
					cout << rows << " " << cnt << endl;
					int debug;
					debug = 1;
					cout << ins.flags() << endl;
				}
			}
			else {
				cout << "didn't handle. loadMat failed." << endl;
				cols /= cn;
			}
			++rows;
		}
		cout << rows << " " << cols << endl;
		ret = cvCreateMat(rows, cols, CV_MAKE_TYPE(CV_32F, cn));
		float *ptr = ret->data.fl;
		CvScalar sca;
		vector<float>::iterator it=data.begin();
		int dataPtr = 0;
		for (int i=0; i<rows; ++i)
			for (int j=0; j<cols; ++j) {
				for (int k=0; k<cn; ++k, ++dataPtr)
					sca.val[k] = data[dataPtr];
				cvSet2D(ret, i, j, sca);
			}
	}

	return ret;
}

void debugByMouseDb(int type, int x, int y, int flag, void* param) {
	switch (type) {
		case CV_EVENT_LBUTTONDOWN:
			CvMat* mat = (CvMat*) param;
			printf("point at (%d, %d) = %lf\n", y, x, CV_MAT_ELEM(*mat, double, y, x));
			break;
	}
}

void debugByMouseCh(int type, int x, int y, int flag, void* param) {
	switch (type) {
		case CV_EVENT_LBUTTONDOWN:
			CvMat* mat = (CvMat*) param;
			if (CV_MAT_CN(mat->type) == 1)
				printf("point at (%d, %d) = %d\n", y, x, CV_MAT_ELEM(*mat, uchar, y, x));
			else
				cout << "point at (" << y << ", " << x << ") = ";
				showRGB(cvGet2D(mat, y, x));
				cout << "img at (" << y << ", " << x << ") = ";
				showRGB(cvGet2D(img, y, x));
			break;
	}
}

CvScalarI cvScalarI(uchar* ptr) {
    CvScalarI res;
    res.val = ptr;
    for (int i=0; i<3; ++i, ++ptr)
        res.data[i] = *ptr;
    return res;
}
CvScalarI cvScalarI(void) {
	CvScalarI res;
	res.val = NULL;
	memset(res.data, 0, 3 * sizeof(int));
	return res;
}
CvScalarI cvScalarI(CvScalar s) {
	CvScalarI res;
	for (int i=0; i<3; ++i)
		res.data[i] = int(s.val[i]);
	res.val = NULL;
	return res;
}

CvScalar cvScalar(CvScalarI s) {
	CvScalar res;
	for (int i=0; i<3; ++i)
		res.val[i] = s.data[i];
	return res;
}

CvMat *cvCopySubMat(CvMat *src, int sx, int sy, int lx, int ly) {
	CvMat *ret = cvCreateMat(lx, ly, src->type);
	for (int i=0; i<lx; ++i)
		for (int j=0; j<ly; ++j)
			cvSet2D(ret, i, j, cvGet2D(src, i+sx, j+sy));
	return ret;
}

void cvSetSubMat(CvMat *tar, int sx, int sy, int lx, int ly, CvMat *src) {
	for (int i=sx; i<min(sx+lx, tar->rows); ++i)
		for (int j=sy; j<min(sy+ly, tar->cols); ++j)
			cvSet2D(tar, i, j, cvGet2D(src, i, j));
}

CvMat* getCn(CvMat* src, int n) {
	int cn = CV_MAT_CN(src->type);
	assert(cn >= n+1);
	CvMat* single;
	single = cvCreateMat(src->rows, src->cols, CV_MAT_DEPTH(src->type));
	switch (n) {
	case 0: cvSplit(src, single, NULL, NULL, NULL); break;
	case 1: cvSplit(src, NULL, single, NULL, NULL); break;
	case 2: cvSplit(src, NULL, NULL, single, NULL); break;
	case 3: cvSplit(src, NULL, NULL, NULL, single); break;
	default: return NULL;
	}
	return single;
}

void putCn(CvMat* dst, CvMat* src, int n) {
	int cn = CV_MAT_CN(dst->type);
	assert(cn >= n+1);
	uchar *dstp = dst->data.ptr + n;
	uchar *srcp = src->data.ptr;
	for (int i=0; i<dst->rows; ++i)
		for (int j=0; j<dst->cols; ++j, dstp += cn, ++srcp)
			*dstp = *srcp;
}

CvMat* gradient(CvMat* src, bool turn8u=false) {
	CvMat *res, *dst;
	dst = cvCreateMat(src->rows,src->cols,CV_16S);
	switch (CV_MAT_CN(src->type)) {
		case 1:
			cvLaplace(src, dst);
			if (turn8u) {
				res = cvCreateMat(src->rows,src->cols,CV_MAT_TYPE(src->type));
				cvConvertScaleAbs(dst , res);
			}
			else
				return dst;
			break;
		case 3:
			if (turn8u) {
				res = cvCreateMat(src->rows,src->cols,CV_MAT_TYPE(src->type));
				CvMat* src1;
				for (int i=0; i<3; ++i) {
					src1 = getCn(src, i);
					cvLaplace(src1, dst);
					cvConvertScaleAbs(dst , src1);
					putCn(res, src1, i);
					cvReleaseMat(&src1);
				}
			}
			else {
				res = cvCreateMat(src->rows, src->cols, CV_16SC3);
				CvMat* src1;
				for (int i=0; i<3; ++i) {
					src1 = getCn(src, i);
					cvLaplace(src1, dst);
					putCn(res, src1, i);
					cvReleaseMat(&src1);
				}
			}
			break;
	}
	cvReleaseMat(&dst);
	return res;
}

template<typename T>
inline void divide2(double *des, T *src, int size, double b) {
	for (int i=0; i<size; ++i,++des,++src)
		*des = *src / b;
}
//only return double
CvMat* divide (CvMat *a, double b) {
	CvMat* res;
	int cn = CV_MAT_CN(a->type);
	res = cvCreateMat(a->rows, a->cols, CV_MAKETYPE(CV_64F, cn));
	double *desp = res->data.db;
	int size = a->rows*a->cols*cn;
	void *srcp = NULL;
	switch (CV_MAT_DEPTH(a->type)) {
		case 0:
		case 1:
			srcp = a->data.ptr;
			divide2(desp, (uchar*)srcp, size, b);
			break;
		case 2:
		case 3:
			srcp = a->data.s;
			divide2(desp, (ushort*)srcp, size, b);
			break;
		case 4:
			srcp = a->data.i;
			divide2(desp, (int*)srcp, size, b);
			break;
		case 5:
			srcp = a->data.fl;
			divide2(desp, (float*)srcp, size, b);
			break;
		case 6:
			srcp = a->data.db;
			divide2(desp, (double*)srcp, size, b);
			break;
	}
	return res;
}

template<typename T>
inline void multi2(uchar *des, T *src, int size, const uchar scale) {
	for (int i=0; i<size; ++i,++des,++src)
		*des = (uchar)(*src * scale);
}
//only return uchar
CvMat* cvMul2 (const uchar scale, const CvMat* src) {
	CvMat* res = NULL;
	int cn = CV_MAT_CN(src->type);
	res = cvCreateMat(src->rows, src->cols, CV_MAKETYPE(CV_8U, cn));
	int size = src->rows*src->cols*cn;
	void *srcp = NULL;
	switch (CV_MAT_DEPTH(src->type)) {
		case CV_8U:
		case CV_8S:
			srcp = src->data.ptr;
			multi2(res->data.ptr, (uchar*)srcp, size, scale);
			break;
		case CV_16U:
		case CV_16S:
			srcp = src->data.s;
			multi2(res->data.ptr, (ushort*)srcp, size, scale);
			break;
		case CV_32S:
			srcp = src->data.i;
			multi2(res->data.ptr, (int*)srcp, size, scale);
			break;
		case CV_32F:
			srcp = src->data.fl;
			multi2(res->data.ptr, (float*)srcp, size, scale);
			break;
		case CV_64F:
			srcp = src->data.db;
			multi2(res->data.ptr, (double*)srcp, size, scale);
			break;
	}
	return res;
}

//only return double
CvMat* cvMul2(CvMat* inSrc1, CvMat* inSrc2, CvMat *dst = NULL, int retType = CV_64F) {
	CvMat* res = NULL;
	CvMat *src1, *src2;
	int cn1 = CV_MAT_CN(inSrc1->type);
	int cn2 = CV_MAT_CN(inSrc2->type);
	src1 = cvCreateMat(inSrc1->rows, inSrc1->cols, CV_MAKETYPE(retType, cn1));
	src2 = cvCreateMat(inSrc2->rows, inSrc2->cols, CV_MAKETYPE(retType, cn2));
	cvConvert(inSrc1, src1);
	cvConvert(inSrc2, src2);

	if (dst == NULL)
		res = cvCreateMat(src1->rows, src2->cols, CV_MAKETYPE(retType, max(cn1, cn2)));
	else
		res = dst;
	if (cn1 == cn2)
		cvMul(src1, src2, res);
	else {
		CvMat *ch1, *ch2, *ch3;
		CvMat *resCh1, *resCh2, *resCh3;
		if (cn1 == 1) {
			ch1 = cvCreateMat(src2->rows, src2->cols, retType);
			ch2 = cvCreateMat(src2->rows, src2->cols, retType);
			ch3 = cvCreateMat(src2->rows, src2->cols, retType);
			cvSplit(src2, ch1, ch2, ch3, NULL);
			resCh1 = cvCreateMat(src2->rows, src2->cols, retType);
			resCh2 = cvCreateMat(src2->rows, src2->cols, retType);
			resCh3 = cvCreateMat(src2->rows, src2->cols, retType);
			cvMul(src1, ch1, resCh1);
			cvMul(src1, ch2, resCh2);
			cvMul(src1, ch3, resCh3);
			cvMerge(resCh1, resCh2, resCh3, NULL, res);
		}
		else {
			ch1 = cvCreateMat(src1->rows, src1->cols, retType);
			ch2 = cvCreateMat(src1->rows, src1->cols, retType);
			ch3 = cvCreateMat(src1->rows, src1->cols, retType);
			cvSplit(src1, ch1, ch2, ch3, NULL);
			resCh1 = cvCreateMat(src1->rows, src1->cols, retType);
			resCh2 = cvCreateMat(src1->rows, src1->cols, retType);
			resCh3 = cvCreateMat(src1->rows, src1->cols, retType);
			cvMul(src2, ch1, resCh1);
			cvMul(src2, ch2, resCh2);
			cvMul(src2, ch3, resCh3);
			cvMerge(resCh1, resCh2, resCh3, NULL, res);
		}
	}
	return res;
}

CvMat* cvConvertMatType(CvMat* src, int type) {
    CvMat* dest;
    dest = cvCreateMat(src->rows, src->cols, type);
    cvConvert(src, dest);
    return dest;
}

CvMat* imread(char name[]) {
	cout << "Loading image: " << name << endl;
	CvMat *tmp;
	tmp = cvLoadImageM(name);

	Mat img;
	img = cv::imread(name);
	imshow("debug", img);
	waitKey(0);

	if (CV_MAT_DEPTH(tmp->type) != CV_8U)
	{
		cout << "image type error!" << endl;
		cvReleaseMat(&tmp);
		return NULL;
	}
	cout << "Loading success" << endl;
	return tmp;
}

CvMat* im2double(CvMat* img) {
	CvMat* tmp;
	tmp = divide(img, 255);
	return tmp;
}

CvMat* rgb2gray(CvMat* img) {
	CvMat *gray;
	gray = cvCreateMat(img->rows, img->cols, CV_8UC1);
	cvCvtColor(img, gray, CV_BGR2GRAY);
	return gray;
}

inline double gaussmf(double x, double sigma, double c = 0) {
	return exp(-sqr(x-c) / (2*sqr(sigma)));
}
inline double cvDotProduct(const CvScalar& src1, const CvScalar& src2) {
	return src1.val[0]*src2.val[0] + src1.val[1]*src2.val[1] +
			src1.val[2]*src2.val[2] + src1.val[3]*src2.val[3];
}
inline int cvDotProduct(const CvScalarI& src1, const CvScalarI& src2) {
    return src1.data[0]*src2.data[0] + src1.data[1]*src2.data[1] + src1.data[2]*src2.data[2];
}
inline bool isEqual(const CvPoint2D64f& x, const CvPoint2D64f& y) {
	if (fabs(x.x - y.x) < 0.000001 && fabs(x.y - y.y) < 0.000001) return true;
	else return false;
}
inline bool isEqual(const CvPoint& x, const CvPoint& y) {
	return (x.x == y.x && x.y == y.y) ? true : false;
}
inline double cvSum3(CvScalar scalar) {
	return scalar.val[0] + scalar.val[1] + scalar.val[2];
}
inline int cvSum3(CvScalarI src) {
    return src.data[0] + src.data[1] + src.data[2];
}
inline int cvSum3(uchar* ptr) {
	return *(ptr) + *(ptr + 1) + *(ptr + 2);
}
inline double cvNorm(const CvScalar& src) {
	return sqrt(sqr(src.val[0]) + sqr(src.val[1]) + sqr(src.val[2]) + sqr(src.val[3]));
}
inline double cvNorm(const CvScalarI& src) {
    return sqrt(double(sqr(src.data[0]) + sqr(src.data[1]) + sqr(src.data[2])));
}
inline double cvDis(const CvPoint& x, const CvPoint& y) {
	return sqrt(double(sqr(x.x-y.x)+sqr(x.y-y.y)));
}
inline CvScalar cvAbs3(CvScalar src) {
	CvScalar res;
	for (int i=0; i<3; ++i)
		res.val[i] = fabs(src.val[i]);
	return res;
}
inline CvScalarI cvAbs3(CvScalarI src) {
    CvScalarI res;
    for (int i=0; i<3; ++i)
        res.data[i] = abs(src.data[i]);
    res.val = src.val;
    return res;
}

inline uchar* cvPtr2D(CvMat* mat, int y, int x) {
    return mat->data.ptr + (size_t)y*mat->step + x*CV_ELEM_SIZE(CV_MAT_TYPE(mat->type));
}
inline uchar* cvPtr1D(CvMat* mat, int idx) {
    return mat->data.ptr + (size_t)idx*CV_ELEM_SIZE(CV_MAT_TYPE(mat->type));
}

CvPoint2D64f operator + (const CvPoint2D64f& x, const CvPoint2D64f& y) {
	CvPoint2D64f res;
	res.x = x.x + y.x;
	res.y = x.y + y.y;
	return res;
}

CvScalar operator - (const CvScalar& src1, const CvScalar& src2) {
	CvScalar res;
	for (int i=0; i<4; ++i)
		res.val[i] = src1.val[i] - src2.val[i];
	return res;
}

CvScalar operator + (const CvScalar& src1, const CvScalar& src2) {
	CvScalar res;
	for (int i=0; i<4; ++i)
		res.val[i] = src1.val[i] + src2.val[i];
	return res;
}

CvScalar operator * (double sca, const CvScalar& src) {
	CvScalar res;
	for (int i=0; i<4; ++i)
		res.val[i] = sca * src.val[i];
	return res;
}

CvScalar operator / (const CvScalar& src, double d) {
	CvScalar res;
	for (int i=0; i<4; ++i)
		res.val[i] = src.val[i] / d;
	return res;
}

bool operator == (const CvScalar& a, const CvScalar& b) {
	for (int i=0; i<4; ++i)
		if (fabs(a.val[i]-b.val[i]) > 0.000001) return false;
	return true;
}

//mat's type must be CV_8UC3
void cvSet8UC3(uchar* ptr, CvScalarI value) {
	for (int i=0; i<3; ++i, ++ptr)
		*ptr = min(value.data[i], 255);
}

void saveRawImg(CvMat *img, char filename[]) {
    FILE* outF;
    fopen_s(&outF, filename, "wb");
    if (CV_MAT_CN(img->type) == 1)
        for (int i=0; i<img->rows; ++i)
            fwrite(CV_MAT_ELEM_PTR(*img, i, 0), CV_ELEM_SIZE(img->type), img->cols, outF);
    else
        for (int i=0; i<img->rows; ++i)
            for (int j=0; j<img->cols; ++j) {
                uchar *ptr = CV_MAT_ELEM_PTR(*img, i, j);
                for (int k=2; k>=0; --k)
                    fwrite((ptr+k), CV_ELEM_SIZE(img->type), 1, outF);
            }
    fclose(outF);
}

void cvSaveMatImage(char filename[], CvMat *mat) {
	int cn = CV_MAT_CN(mat->type);
	int imgType = -1;
    CvMat *savMat;
	switch (CV_MAT_TYPE(mat->type)%8) {
	case CV_8U: savMat = mat; imgType = IPL_DEPTH_8U; break;
	case CV_16S: savMat = mat; imgType = IPL_DEPTH_16S; break;
	case CV_32F: 
	case CV_64F: 
        CvMat *tmpMat = cvCreateMat(mat->rows, mat->cols, CV_MAKETYPE(CV_MAT_TYPE(mat->type), cn));
        cvScale(mat, tmpMat, 255);
        savMat = cvConvertMatType(tmpMat, CV_MAKETYPE(CV_8U, cn));
        cvReleaseMat(&tmpMat);
        imgType = IPL_DEPTH_8U;
	}
	IplImage *header = cvCreateImageHeader(cvSize(mat->rows, mat->cols), imgType, cn);
	cvSaveImage(filename, cvGetImage(savMat, header));
}
//=========================================================================================

//img must be one channel
CvMat* RefineMask(CvMat* img) {
	uchar* p = img->data.ptr;
	for (int i=0; i<img->rows*img->step; ++i, ++p) {
		if (*p == 0 || *p == 127 || *p == 255) continue;
		else if (*p < 64) *p = 0;
		else if (*p < 196) *p = 127;
		else *p = 255;
	}
	return img;
}

//un2pos must set to 1 if need it.
CvPointList* analyseTrimap(CvMat* mask, CvPointList* fg, CvPointList* bg, int** un2pos = NULL) {
	uchar *p = mask->data.ptr;
	int fc, bc, uc;
	fc = bc = uc = 0;
	
	for (int i=0; i<mask->rows; ++i, p+=mask->step-mask->cols)
        for (int j=0; j<mask->cols; ++j, ++p)
    		if (*p == 0) ++bc;
	    	else if (*p == 255) ++fc;
		    else if (*p == 127) ++uc;

	if (fg != NULL) {
		fg->data = new CvPoint[fc];
		fg->length = fc;
	}
	if (bg != NULL) {
		bg->data = new CvPoint[bc];
		bg->length = bc;
	}
	CvPointList *unknown = new CvPointList;
	unknown->data = new CvPoint[uc];
	unknown->length = uc;

	bc = fc = uc = 0;
	p = mask->data.ptr;
	for (int i=0; i<mask->rows; ++i)
		for (int j=0; j<mask->cols; ++j, ++p) {
			if (*p == 127) {
				if (un2pos != NULL)
					un2pos[i][j] = uc;
				unknown->data[uc++] = cvPoint(i, j);
			}
			else if (bg != NULL && *p == 0) bg->data[bc++] = cvPoint(i, j);
			else if (fg != NULL) fg->data[fc++] = cvPoint(i, j);
		}
	long endTime = clock();
	return unknown;
}



void getSampleSet(const CvPoint& center, CvMat* imgMask, int lineNum, CvPointList* fset, CvPointList* bset, double inTheta = -1) {
#ifdef _DEBUG
	double theta = ((center.x%5*5 + center.y%5) * 3.5 + 5%3) / 180.0 * M_PI;
#else
    double theta = ((center.x%5*5 + center.y%5) * 3.5 + rand()%3) / 180.0 * M_PI;
#endif
	double incTheta = 2 * M_PI / lineNum;
	CvPoint2D64f dir, cur;
	CvPoint tmpIntp, last;
	bool meetB, meetF;

// 	if (center.x == 347 && center.y == 502) {
// 		int debug;
// 		debug = 1;
// 	}

	fset->length = 0;
	bset->length = 0;

	if (inTheta >= 0) 
		theta = inTheta;
	for (int i=0; i<lineNum; ++i) {
		dir.x = cos(theta)*3;
		dir.y = sin(theta)*3;
		cur.x = center.x;
		cur.y = center.y;
		last.x = last.y = -1;
		meetF = meetB = false;
		for (int j=0; j<300; ++j) {
			cur = cur + dir;
			tmpIntp.x = int(cur.x+0.5);
			tmpIntp.y = int(cur.y+0.5);
			if (tmpIntp.x < 0 || tmpIntp.x >= imgMask->rows || 
				tmpIntp.y < 0 || tmpIntp.y >= imgMask->cols)
				break;
			if (isEqual(tmpIntp, last)) continue;
			if (!meetB && CV_MAT_ELEM(*imgMask, uchar, tmpIntp.x, tmpIntp.y) == 0) {
				meetB = true;
				bset->data[bset->length++] = tmpIntp;
			}
			if (!meetF && CV_MAT_ELEM(*imgMask, uchar, tmpIntp.x, tmpIntp.y) == 255) {
				meetF = true;
				fset->data[fset->length++] = tmpIntp;
			}
			if (meetB && meetF) break;
			last = tmpIntp;
		}
		theta += incTheta;
	}
}

CvPoint getSampleSet2(const CvPoint& start, const CvPoint& end, CvMat* imgMask, int val) {
	CvLineIterator iter;
	CvPoint newStart, newEnd, res;
	newStart.x = start.y;
	newStart.y = start.x;
	newEnd.x = end.y;
	newEnd.y = end.x;
	res.x = res.y = -1;

	int cnt = cvInitLineIterator(imgMask, newStart, newEnd, &iter);
	for (int i=0; i<cnt; ++i) {
		if (*iter.ptr == val) {
			int offset;
			offset = int(iter.ptr - imgMask->data.ptr);
			res.x = offset / imgMask->step; //need to swap x, y cause different coordinate
			res.y = offset % imgMask->step;
			break;
		}
		CV_NEXT_LINE_POINT(iter);
	}
//     if (res.x == -1) {
//         int debug = 1;
//         debug = 1;
//         cout << int(CV_MAT_ELEM(*imgMask, uchar, end.x, end.y)) << endl;
//     }
	return res;
}

int eFunc(const CvPoint& start, const CvPoint& end, CvMat* grad) {
    int energy = 0;
    CvLineIterator iter;
    CvPoint newStart, newEnd;
    newStart.x = start.y;
    newStart.y = start.x;
    newEnd.x = end.y;
    newEnd.y = end.x;

    int cnt = cvInitLineIterator(grad, newStart, newEnd, &iter);
    for (int i=0; i<cnt; ++i) {
        energy += cvSum3(iter.ptr);
        CV_NEXT_LINE_POINT(iter);
    }
    return energy;
}

double PFFunc(const CvPoint& center, CvPointList *fset, CvPointList *bset, CvMat *gradImg) {
	if (bset->length == 0 || fset->length == 0) {
#ifdef _DEBUG
//		cout << "no candidate sample point. set PF = 0.5" << endl;
#endif
		return 0.5;
	}
	int mineb, minef, eb, ef;
	mineb = INT_MAX;
	for (int i=0; i<bset->length; ++i) {
		eb = eFunc(center, bset->data[i], gradImg);
		mineb = mineb > eb ? eb : mineb;
	}
	minef = INT_MAX;
	for (int i=0; i<fset->length; ++i) {
		ef = eFunc(center, fset->data[i], gradImg);
		minef = minef > ef ? ef : minef;
	}
	return double(mineb) / (minef + mineb);
}

inline double roughAlpha(const CvScalar& f, const CvScalar& b, const CvScalar& q) {
	return max(0., min(1., (f == b) ? 0.5 : cvDotProduct(f - b, q - b) / pow(cvNorm(f - b), 2)));
}
inline double roughAlpha(const CvScalarI& f, const CvScalarI& b, const CvScalarI& q) {
    return max(0., min(1., (f == b) ? 0.5 : cvDotProduct(f - b, q - b) / pow(cvNorm(f - b), 2)));
}

double mFunc(const CvScalar& c, const CvScalar& f, const CvScalar& b) {
	double alpha = roughAlpha(f, b, c);
	return cvNorm(c - (alpha*f + (1-alpha)*b)) / 255;
}
double mFunc(const CvScalarI& c, const CvScalarI& f, const CvScalarI& b) {
    double alpha = roughAlpha(f, b, c);
    return cvNorm(c - (f*alpha + b*(1-alpha))) / 255;
}

double nFunc(const CvPoint& center, const CvPoint& fg, const CvPoint& bg, CvMat* img) {
	double res = 0;
    CvScalarI fc, bc, ic;
    fc = cvScalarI(cvPtr2D(img, fg.x, fg.y));
    bc = cvScalarI(cvPtr2D(img, bg.x, bg.y));
    for (int i=max(0, center.x-1); i<min(img->rows, center.x+1); ++i) {
        ic = cvScalarI(cvPtr2D(img, i, max(0, center.y-1)));
		for (int j=max(0, center.y-1); j<min(img->cols, center.y+1); ++j, ++ic) {
			res += pow(mFunc(ic, fc, bc), 2);
		}
    }
	return res;
}

inline double aFunc(const CvPoint& center, const CvPoint& fg, const CvPoint& bg, double pf, CvMat* img) {
	return pf + (1 - 2*pf) * roughAlpha(cvScalarI(cvPtr2D(img, fg.x, fg.y)), 
			cvScalarI(cvPtr2D(img, bg.x, bg.y)),
			cvScalarI(cvPtr2D(img, center.x, center.y)));
}

void calSigma(CvPoint minp, CvMat* sigma, int index, CvMat* img, bool debug = false) {
	double sumSigma = 0;
	int len = 0;
	CvScalarI centerColor, color;
	centerColor = cvScalarI(cvPtr2D(img, minp.x, minp.y));
	for (int i=max(0, minp.x-3); i<min(img->rows, minp.x+3); ++i) {
		color = cvScalarI(cvPtr2D(img, i, max(0, minp.y-3)));
		for (int j=max(0, minp.y-3); j<min(img->cols, minp.y+3); ++j, ++color) {
			sumSigma += cvNorm(color - centerColor);
			++len;
		}
	}
	cvSetReal1D(sigma, index, sumSigma/len);
}
// void calSigma(CvPoint minp, CvMat* sigma, int index, CvMat* img) {
// 	double sumSigma = 0, tmp;
// 	int len = 0, cnt = 0;
// 	CvScalarI centerColor, color;
// 	centerColor = cvScalarI(cvPtr2D(img, minp.x, minp.y));
// 	for (int i=max(0, minp.x-3); i<min(imgMask->rows, minp.x+3); ++i) {
// 		color = cvScalarI(cvPtr2D(img, i, max(0, minp.y-3)));
// 		for (int j=max(0, minp.y-3); j<min(imgMask->cols, minp.y+3); ++j, ++color) {
// 			tmp = cvNorm(color - centerColor);
// 			if (tmp < 10) {
// 				++cnt;
// 				sumSigma += tmp;
// 			}
// 			++len;
// 		}
// 	}
// 	if (cnt > (len >> 1))
// 		cvSetReal1D(sigma, index, sumSigma / cnt);
// 	else
// 		cvSetReal1D(sigma, index, 0);
// }

CvMat* pyrDownMat(CvMat* src, int rowStart = 0, int colStart = 0) {
    CvMat* res;
    res = cvCreateMat((src->rows+1-rowStart)/2, (src->cols+1-colStart)/2, src->type);
    int cn = CV_MAT_CN(src->type);
    uchar* srcPtr;
    uchar* resPtr;
    for (int i=rowStart; i<src->rows; i+=2) {
        srcPtr = src->data.ptr + i*src->step + cn*colStart;
        resPtr = res->data.ptr + i/2*res->step;
        for (int j=colStart; j<src->cols; j+=2) {
            memcpy(resPtr, srcPtr, cn);
            resPtr += cn;
            srcPtr += cn<<1;
        }
    }
    return res;
}

typedef struct SelectionPara {
    CvMat* halfImg;
    CvMat* halfImgMask;
    CvMat* halfGradImg;
    int lineNum;
    CvMat* oselectf;
    CvMat* oselectb;
    CvMat* osigmaf;
    CvMat* osigmab;
    CvPointList* unknown;
    CvPointList* fgPoint;
    CvPointList* bgPoint;
    int rowStart;
    int colStart;
    int unknownStartPos;
    int start;
    int end;
} SelectionPara;

void sampleSelectionK(SelectionPara* param) {
    CvPointList fset, bset;
    double pf, n, a, df, db, cost, mincost;
    int en = 3, ea = 2, ef = 1, eb = 4;
    double wh = param->halfImg->cols + param->halfImg->rows;
    CvPoint minf, minb;
    double DBL_MAX_10 = DBL_MAX / 10;
    uchar* selfPtr = param->oselectf->data.ptr + 3*(param->unknownStartPos + param->start);
    uchar* selbPtr = param->oselectb->data.ptr + 3*(param->unknownStartPos + param->start);
    CvScalarI fc, bc, centerColor, ic;
    int startJJ;

	fset.data = new CvPoint[param->lineNum];
	bset.data = new CvPoint[param->lineNum];

    for (int i=param->start; i<param->end; ++i, selfPtr+=param->oselectf->step, selbPtr+=param->oselectb->step) {

        centerColor = cvScalarI(cvPtr2D(param->halfImg, param->unknown->data[i].x, param->unknown->data[i].y));
        getSampleSet(param->unknown->data[i], param->halfImgMask, param->lineNum, &fset, &bset);
		if (fset.length == 0) {
			CvPoint sf = param->fgPoint->data[rand()%param->fgPoint->length];
			fset.data[fset.length++] = getSampleSet2(param->unknown->data[i], sf, param->halfImgMask, 255);
		}
		if (bset.length == 0) {
			CvPoint sf = param->bgPoint->data[rand()%param->bgPoint->length];
			bset.data[bset.length++] = getSampleSet2(param->unknown->data[i], sf, param->halfImgMask, 0);
		}

        pf = PFFunc(param->unknown->data[i], &fset, &bset, param->halfGradImg);
        mincost = DBL_MAX;
        minf.x = minf.y = minb.x = minb.y = -1;
        for (int j=0; j<fset.length; ++j) {
            fc = cvScalarI(cvPtr2D(param->halfImg, fset.data[j].x, fset.data[j].y));
            for (int k=0; k<bset.length; ++k) {
                bc = cvScalarI(cvPtr2D(param->halfImg, bset.data[k].x, bset.data[k].y));

                n = 0;
                for (int ii=max(0, param->unknown->data[i].x-1); ii<min(param->halfImg->rows, param->unknown->data[i].x+1); ++ii) {
                    startJJ = max(0, param->unknown->data[i].y-1);
                    ic = cvScalarI(cvPtr2D(param->halfImg, ii, startJJ));
                    for (int jj=startJJ; jj<min(param->halfImg->cols, param->unknown->data[i].y+1); ++jj, ++ic) {
                        n += pow(mFunc(ic, fc, bc), 2);
                    }
                }

                a = pf + (1 - 2*pf) * roughAlpha(fc, bc, centerColor);

                df = (abs(param->unknown->data[i].x - fset.data[j].x) + abs(param->unknown->data[i].y - fset.data[j].y)) / wh;
                db = (abs(param->unknown->data[i].x - bset.data[k].x) + abs(param->unknown->data[i].y - bset.data[k].y)) / wh;
                cost = pow(n, en) + pow(a, ea) + pow(df, ef) + pow(db, eb);
                if (cost < mincost) {
                    mincost = cost;
                    minf = fset.data[j];
                    minb = bset.data[k];
                }
            }
        }

        //pyrUp
        unknownList->data[i+param->unknownStartPos].x = (param->unknown->data[i].x<<SPLIT) + param->rowStart;
        unknownList->data[i+param->unknownStartPos].y = (param->unknown->data[i].y<<SPLIT) + param->colStart;

        if (mincost > DBL_MAX_10) {
			unsampled.data[unsampled.length++] = param->unknown->data[i];

			if (fset.length == 0 && bset.length != 0) {
				mincost = DBL_MAX;
				for (int j=0; j<bset.length; ++j) {
					bc = cvScalarI(cvPtr2D(param->halfImg, bset.data[j].x, bset.data[j].y));
					cost = cvNorm(cvAbs3(centerColor - bc));
					if (cost < mincost) {
						mincost = cost;
						minb = bset.data[j];
					}
				}

				memcpy(selbPtr, cvPtr2D(param->halfImg, minb.x, minb.y), 3);
				memset(selfPtr, 0, 3);
				*selfPtr = *(selfPtr+1) = *(selfPtr+2) = 127;
				calSigma(minb, param->osigmab, i+param->unknownStartPos, param->halfImg);
				cvSetReal1D(param->osigmaf, i+param->unknownStartPos, DBL_MAX);
			}
			if (bset.length == 0 && fset.length != 0) {
				mincost = DBL_MAX;
				for (int j=0; j<fset.length; ++j) {
					fc = cvScalarI(cvPtr2D(param->halfImg, fset.data[j].x, fset.data[j].y));
					cost = cvNorm(cvAbs3(centerColor - fc));
					if (cost < mincost) {
						mincost = cost;
						minf = fset.data[j];
					}
				}

				memcpy(selfPtr, cvPtr2D(param->halfImg, minf.x, minf.y), 3);
				*selbPtr = *(selbPtr+1) = *(selbPtr+2) = 127;
				calSigma(minf, param->osigmaf, i+param->unknownStartPos, param->halfImg);
				cvSetReal1D(param->osigmab, i+param->unknownStartPos, DBL_MAX);
			}

            continue;
        }

        memcpy(selfPtr, cvPtr2D(param->halfImg, minf.x, minf.y), 3);
        memcpy(selbPtr, cvPtr2D(param->halfImg, minb.x, minb.y), 3);

        calSigma(minf, param->osigmaf, i+param->unknownStartPos, param->halfImg);
        calSigma(minb, param->osigmab, i+param->unknownStartPos, param->halfImg);
    }
	delete[] fset.data;
	delete[] bset.data; 
}

void sampleSelection(CvMat* img, CvMat* imgMask, int lineNum,
					 CvMat*& oselectf, CvMat*& oselectb, CvMat*& osigmaf, CvMat*& osigmab) {
	long startTime = clock();
	if (verbose)
		cout << "in sample selection" << endl;

	CvMat* gradImg = gradient(img);

    oselectf = cvCreateMat(unknownList->length, 1, CV_8UC3);
    oselectb = cvCreateMat(unknownList->length, 1, CV_8UC3);
    osigmaf = cvCreateMat(unknownList->length, 1, CV_64F);
    osigmab = cvCreateMat(unknownList->length, 1, CV_64F);
    int unknownStartPos = 0;

	CvMat *halfImg, *halfImgMask, *halfGradImg;
    for (int rowStart = 0; rowStart < SPLIT+1; ++rowStart)
        for (int colStart = 0; colStart < SPLIT+1; ++colStart) {
			if (SPLIT == 0) {
				halfImg = cvCloneMat(img);
				halfImgMask = cvCloneMat(imgMask);
				halfGradImg = cvCloneMat(gradImg);
			}
			else {
				halfImg = pyrDownMat(img, rowStart, colStart);
				halfImgMask = pyrDownMat(imgMask, rowStart, colStart);
				halfGradImg = pyrDownMat(gradImg, rowStart, colStart);
			}
            int** un2pos;
            un2pos = new int*[halfImgMask->rows];
            for (int i=0; i<halfImgMask->rows; ++i) {
                un2pos[i] = new int[halfImgMask->cols];
                for (int j=0; j<halfImgMask->cols; ++j)
                    un2pos[i][j] = -1;
            }
            CvPointList* unknown = analyseTrimap(halfImgMask, NULL, NULL, un2pos);
            //pyrUp
            for (int i=0; i<halfImgMask->rows; ++i)
                for (int j=0; j<halfImgMask->cols; ++j)
                    if (un2pos[i][j] != -1)
                        un2PosMap[(i<<SPLIT)+rowStart][(j<<SPLIT)+colStart] = un2pos[i][j]+unknownStartPos;


            double a, b = 0, step = unknown->length / double(pn) + 0.001;
            SelectionPara *param;
            param = new SelectionPara[pn];
            HANDLE *ht;
            ht = new HANDLE[pn];
            for (int i=0; i<pn; ++i) {
                a = b;
                b += step;
                param[i].halfImg= halfImg;
                param[i].halfImgMask= halfImgMask;
                param[i].halfGradImg = halfGradImg;
                param[i].lineNum = lineNum;
                param[i].oselectf = oselectf;
                param[i].oselectb = oselectb;
                param[i].osigmaf = osigmaf;
                param[i].osigmab = osigmab;
                param[i].unknown = unknown;
                param[i].fgPoint = &fgPoint[(rowStart<<SPLIT)+colStart];
                param[i].bgPoint = &bgPoint[(rowStart<<SPLIT)+colStart];
                param[i].rowStart = rowStart;
                param[i].colStart = colStart;
                param[i].unknownStartPos = unknownStartPos;
                param[i].start = int(a);
                param[i].end = int(b);
                ht[i] = CreateThread(NULL, 0, (LPTHREAD_START_ROUTINE)sampleSelectionK, &param[i], 0, NULL);
            }

            WaitForMultipleObjects(pn, ht, TRUE, 60000);
            for (int i=0; i<pn; ++i)
                CloseHandle(ht[i]);
            delete[] param;
            delete[] ht;

            unknownStartPos += unknown->length;

            for (int i=0; i<halfImgMask->rows; ++i)
                delete[] un2pos[i];
            delete[] un2pos;
            delete[] unknown;
            cvReleaseMat(&halfImg);
            cvReleaseMat(&halfImgMask);
            cvReleaseMat(&halfGradImg);
        }

	cvReleaseMat(&gradImg);
	if (verbose) {
		long endTime = clock();
		cout << "selection time: " << double(endTime - startTime) / CLOCKS_PER_SEC << endl;
	}
}

CvScalar avgScalar(CvMat* src, int size, int* index, double* weight) {
	CvScalar sum = cvScalar(0);
	int cnt = 0;
	for (int j=0; j<size; ++j, ++cnt)
		if (index[j] != -1)
			sum = sum + weight[j]*cvGet1D(src, index[j]);
		else
			break;
	return sum;
}

CvScalarI avgScalarI(CvMat* src, int size, int* index) {
	CvScalarI sum = cvScalarI();
	int cnt = 0;
	for (int j=0; j<size; ++j, ++cnt)
		if (index[j] != -1)
			sum = sum + cvScalarI(cvPtr1D(src, index[j]));
		else
			break;
	return sum / cnt;
}

double avgSig(CvMat* src, int size, int* index) {
	double sum = 0;
	int cnt = 0;
	for (int j=0; j<size; ++j, ++cnt)
		if (index[j] != -1)
			sum += cvGetReal1D(src, index[j]);
		else
			break;
	sum /= cnt;
	return sum;
}

bool tooDiff(CvScalar x, CvScalar y, int diff = 30) {
	if (cvSum3(cvAbs3(x - y)) > diff)
		return true;
	else
		return false;
}

bool tooDiff(CvScalarI x, CvScalarI y, int diff = 60) {
	if (cvNorm(x - y) > diff)
		return true;
	else
		return false;
}

typedef struct RefinementPara {
    CvMat* img;
    CvMat* imgMask;
    CvMat* inf;
    CvMat* inb;
    CvMat* inSigf;
    CvMat* inSigb;

    uchar* outfPtr;
    uchar* outbPtr;
    double* outaPtr;
    double* outcPtr;
    CvPointList* unknown;
    int **un2pos;
    int outfStep;
    int outbStep;
    int start;
    int end;
} RefinementPara;

void sampleRefinementK(RefinementPara* param) {
    CvPointList *unknown = unknownList;
    double *forSortAlpha = new double[unknown->length];
    int *forSortInd = new int[unknown->length];
    int cnt;
    CvScalarI centerColor, color;
    double epsilon = 0.0000001;
    double lamda = 10;

    for (int i=param->start; i<param->end; ++i, param->outfPtr+=param->outfStep, param->outbPtr += param->outbStep,
        ++param->outaPtr, ++param->outcPtr) {
//             if (param->unknown->data[i].x==19 && param->unknown->data[i].y==223) {
//                 CvScalarI inf = cvScalarI(cvGet1D(param->inf, i));
//                 CvScalarI inb = cvScalarI(cvGet1D(param->inb, i));
//                 int debug;
//                 debug = 1;
//             }

            centerColor = cvScalarI(cvPtr2D(param->img, param->unknown->data[i].x, param->unknown->data[i].y));
            cnt = 0;
            for (int j=max(0, param->unknown->data[i].x-3); j<min(param->imgMask->rows, param->unknown->data[i].x+3); ++j) {
                color = cvScalarI(cvPtr2D(param->img, j, max(0, param->unknown->data[i].y-3)));
                for (int k=max(0, param->unknown->data[i].y-3); k<min(param->imgMask->cols, param->unknown->data[i].y+3); ++k, ++color) {
                    int idx = param->un2pos[j][k];
                    if (idx == -1) continue;// || tooDiff(centerColor, color, 30)) continue;
                    else {
                        forSortInd[cnt] = idx;
                        forSortAlpha[cnt++] = mFunc(centerColor, cvScalarI(cvPtr1D(param->inf, idx)), 
                            cvScalarI(cvPtr1D(param->inb, idx)));
                    }
                }
            }
            double minValue;

            int top3[3];
            top3[0] = top3[1] = top3[2] = -1;
            for (int j=0; j<3; ++j) {
                minValue = DBL_MAX;
                for (int k=0; k<cnt; ++k)
                    if (forSortAlpha[k] < minValue && k != top3[0] && k != top3[1]) {
                        minValue = forSortAlpha[k];
                        top3[j] = k;
                    }
            }

            double tmpw[3], top3w [3];
            tmpw[0] = tmpw[1] = tmpw[2] = 0;
            for (int j=0; j<3; ++j) {
                if (top3[j] != -1) {
                    tmpw[j] = forSortAlpha[top3[j]];
                    top3[j] = forSortInd[top3[j]];
                }
            }
            //belong code for add weights to top3 alpha
            const int INC_PRECISE = 10000;
            top3w[0] = top3w[1] = top3w[2] = INC_PRECISE;
            for (int j=0; j<3; ++j) {
                if (top3[j] != -1)
                    for (int k=0; k<3; ++k)
                        if (k != j) {
                            top3w[k] *= tmpw[j];
                        }
            }
            double sumw = 0;
            for (int j=0; j<3; ++j)
                if (top3[j] != -1)
                    sumw += ++top3w[j];
            for (int j=0; j<3; ++j)
                if (top3[j] != -1)
                    top3w[j] /= sumw;


            CvScalarI savOutf = cvScalarI(avgScalar(param->inf, 3, top3, top3w));
            CvScalarI savOutb = cvScalarI(avgScalar(param->inb, 3, top3, top3w));
            double m2 = mFunc(centerColor, savOutf, savOutb);

            //belong code can be optimized, here just for easy debug
            top3[0] = i;
            double tsf = avgSig(param->inSigf, 1, top3);
            double tsb = avgSig(param->inSigb, 1, top3);
            double disF = cvNorm(centerColor - savOutf);
            double disB = cvNorm(centerColor - savOutb);

            if (disF <= min(tsf, 0.15*255))
                cvSet8UC3(param->outfPtr, centerColor);
            else
                cvSet8UC3(param->outfPtr, savOutf);
            if (disB <= min(tsb, 0.15*255))
                cvSet8UC3(param->outbPtr, centerColor);
            else
                cvSet8UC3(param->outbPtr, savOutb);

            *param->outaPtr = max(0., min(1., roughAlpha(cvScalarI(param->outfPtr), cvScalarI(param->outbPtr), centerColor)));
            if (abs(cvSum3(cvScalarI(param->outfPtr) - cvScalarI(param->outbPtr))) < 5)
                *param->outcPtr = epsilon;
            else
                *param->outcPtr = exp(-lamda * mFunc(centerColor, savOutf, savOutb));
    }

	delete[] forSortInd;
	delete[] forSortAlpha;
}

void sampleRefinement(CvMat* inf, CvMat* inb, CvMat* inSigf, CvMat* inSigb, CvMat* img, CvMat* imgMask, int nearNum,
					  CvMat*& outf, CvMat*& outb, CvMat*& outAlpha, CvMat*& outConf) {
	if (verbose)
		cout << "in refinement" << endl;
	long startTime = clock();
	int** un2pos;
// 	un2pos = new int*[imgMask->rows];
// 	for (int i=0; i<imgMask->rows; ++i) {
// 		un2pos[i] = new int[imgMask->cols];
// 		for (int j=0; j<imgMask->cols; ++j)
// 			un2pos[i][j] = -1;
// 	}
	//CvPointList *unknown = analyseTrimap(imgMask, NULL, NULL, un2pos);
    CvPointList *unknown = unknownList;
    un2pos = un2PosMap;
	outf = cvCreateMat(inf->rows, 1, inf->type);
	outb = cvCreateMat(inb->rows, 1, inb->type);
	outAlpha = cvCreateMat(inf->rows, 1, CV_64F);
	outConf = cvCreateMat(inf->rows, 1, CV_64F);

    double a, b = 0, step = unknown->length / double(pn) + 0.001;
    RefinementPara *param;
    param = new RefinementPara[pn];
    HANDLE *ht;
    ht = new HANDLE[pn];
    for (int i=0; i<pn; ++i) {
        a = b;
        b += step;
        param[i].img= img;
        param[i].imgMask= imgMask;
        param[i].inf = inf;
        param[i].inb = inb;
        param[i].inSigf = inSigf;
        param[i].inSigb = inSigb;
        param[i].outfPtr = outf->data.ptr + 3*int(a);
        param[i].outbPtr = outb->data.ptr + 3*int(a);
        param[i].outaPtr = outAlpha->data.db + 1*int(a);
        param[i].outcPtr = outConf->data.db + 1*int(a);
        param[i].unknown = unknown;
        param[i].un2pos = un2pos;
        param[i].outfStep = outf->step;
        param[i].outbStep = outb->step;
        param[i].start = int(a);
        param[i].end = int(b);
        ht[i] = CreateThread(NULL, 0, (LPTHREAD_START_ROUTINE)sampleRefinementK, &param[i], 0, NULL);
    }
    WaitForMultipleObjects(pn, ht, TRUE, 60000);
    for (int i=0; i<pn; ++i)
        CloseHandle(ht[i]);

	if (verbose) {
		long endTime = clock();
		cout << "refinement time: " << double(endTime - startTime) / CLOCKS_PER_SEC << endl;
	}
}

CvMat* calWholeMat(CvMat* unF, CvMat* unB, CvMat* unAlpha, CvMat* unConf, CvMat* img, CvMat* imgMask,
				   CvMat*& fMat, CvMat*& bMat, CvMat*& confMat) {
	long startTime = clock();
	if (verbose)
		cout << "in cal whole mat" << endl;
	CvMat* alphaMat;
	fMat = cvCloneMat(img);
	bMat = cvCloneMat(img);
	alphaMat = cvCreateMat(imgMask->rows, imgMask->cols, CV_64F);
	confMat = cvCreateMat(imgMask->rows, imgMask->cols, CV_64F);
	int** un2pos;
// 	un2pos = new int*[imgMask->rows];
// 	for (int i=0; i<imgMask->rows; ++i) {
// 		un2pos[i] = new int[imgMask->cols];
// 	}
// 	analyseTrimap(imgMask, NULL, NULL, un2pos);
    un2pos = un2PosMap;
	uchar* maskPtr = imgMask->data.ptr;
	double* alphaDb = alphaMat->data.db;
	double* confDb = confMat->data.db;
	for (int i=0; i<imgMask->rows; ++i)
		for (int j=0; j<imgMask->cols; ++j, ++maskPtr, ++alphaDb, ++confDb) {
// 			if (i == 248 && j == 13) {
// 				int debug;
// 				debug = 1;
// 			}
			if (*maskPtr == 0) {
				*alphaDb = 0;
				*confDb = 1;
			}
			else if (*maskPtr == 255) {
				*alphaDb = 1;
				*confDb = 1;
			}
			else if (*maskPtr == 127) {
				cvSet2D(bMat, i, j, cvGet1D(unB, un2pos[i][j]));
				cvSet2D(fMat, i, j, cvGet1D(unF, un2pos[i][j]));
				*alphaDb = cvGetReal1D(unAlpha, un2pos[i][j]);
				*confDb = cvGetReal1D(unConf, un2pos[i][j]);
			}
			else {
				cout << "bad img mask." << endl;
			}
		}
	if (verbose) {
		long endTime = clock();
		cout << "cal mat time: " << double(endTime - startTime) / CLOCKS_PER_SEC << endl;
	}
	return alphaMat;
}

typedef struct GaussianDis {
    double **data;
    int size;
} GaussianDis;

GaussianDis gaussianDis(int size) {
    GaussianDis g;
    if (size % 2 == 0)
        ++size;
    g.size = size;
    g.data = new double*[size];
    for (int i=0; i<size; ++i)
        g.data[i] = new double[size];
    return g;
}
void releaseGD(GaussianDis *gd) {
    for (int i=0; i<gd->size; ++i)
        delete[] gd->data[i];
    delete[] gd->data;
    gd->size = 0;
    gd->data = 0;
}

CvMat* localSmoothing(CvMat *inF, CvMat *inB, CvMat *inAlpha, CvMat *inConf, CvMat* img, CvMat *imgMask, int limitNum) {
	long startTime = clock();
	if (verbose)
		cout << "in smoothing" << endl;
	CvPointList* unknown;
	CvPointList neighbors;
	CvPoint neighbor;
	CvMat* finalAlpha;
	double sigma, g, wc, wfb, disFB, wa;
	int lamda, cnt;
	CvScalar fUp, bUp;
	CvScalarI centerColor, color, inFColor, inBColor, f, b;
	double fDown, bDown, dfbUp, dfbDown, aUp, aDown, dfb, conf, alpha;
	double *aPtr, *cPtr, centerAlpha;
	uchar *maskPtr;
//	unknown = analyseTrimap(imgMask, NULL, NULL);
    unknown = unknownList;
	finalAlpha = cvCloneMat(inAlpha);
	sigma = limitNum / (9 * M_PI);
	lamda = 5;
	double alphaCoeff;
    int colorDis;
    int offsetX, offsetY;
    offsetX = offsetY = 4;
	
    GaussianDis gf = gaussianDis(9);
    for (int i=-4; i<5; ++i)
        for (int j=-4; j<5; ++j) {
            CvPoint center = cvPoint(0, 0);
            CvPoint neighbor = cvPoint(i, j);
            gf.data[i+offsetX][j+offsetX] = gaussmf(cvDis(center, neighbor), sigma);
        }

	double tmpA = 0;

    for (int i=0; i<unknown->length; ++i) {
		bUp = fUp = cvScalar(0);
		bDown = fDown = dfbDown = dfbUp = aUp = aDown = 0;
		cnt = 0;
		neighbors.length = 0;
		centerColor = cvScalarI(cvPtr2D(img, unknown->data[i].x, unknown->data[i].y));
		centerAlpha = cvGetReal2D(inAlpha, unknown->data[i].x, unknown->data[i].y);
		for (int j=max(0, unknown->data[i].x-4); j<min(img->rows, unknown->data[i].x+5); ++j) {
			int startK = max(0, unknown->data[i].y-4);
			color = cvScalarI(cvPtr2D(img, j, startK));
			aPtr = (double*)cvPtr2D(inAlpha, j, startK);
			cPtr = (double*)cvPtr2D(inConf, j, startK);
			inFColor = cvScalarI(cvPtr2D(inF, j, startK));
			inBColor = cvScalarI(cvPtr2D(inB, j, startK));
			maskPtr = cvPtr2D(imgMask, j, startK);

			for (int k=startK; k<min(img->cols, unknown->data[i].y+5); ++k, ++cnt, ++color,
					++aPtr, ++cPtr, ++inFColor, ++inBColor, ++maskPtr) {
                colorDis = cvSum3(cvAbs3(color - centerColor));
                if (colorDis > 90) continue;
                neighbor.x = j;
                neighbor.y = k;
				alphaCoeff = -exp(colorDis/22.5056-4)+1;
				g = gf.data[offsetX + unknown->data[i].x-j][offsetY + unknown->data[i].y-k];
//                 g = gaussmf(cvDis(unknown->data[i], neighbor), sigma);
// 				double g2;
//                 g2 = gf.data[offsetX + unknown->data[i].x-j][offsetY + unknown->data[i].y-k];
// 				cout << g << " " << g2 << endl;
				tmpA = *aPtr;
				if (isEqual(unknown->data[i], neighbor))
					wc = g * (*cPtr);
				else
					wc = g * (*cPtr) * fabs(centerAlpha - tmpA);
				wfb = (*cPtr) * tmpA * (1 - tmpA);
				disFB = cvNorm(inFColor - inBColor);
				wa = g * (*cPtr) + (*maskPtr != 127);

				wc *= alphaCoeff;
				wfb *= alphaCoeff;
				wa *= alphaCoeff;

				fUp = fUp + inFColor * (wc * tmpA);
				fDown += wc * tmpA;
				bUp = bUp + inBColor * (wc * (1-tmpA));
				bDown += wc * (1-tmpA);
				dfbUp += wfb * disFB;
				dfbDown += wfb;
				aUp += wa * tmpA;
				aDown += wa;
			}
		}
		f = cvScalarI(fUp / (fDown + 0.000001));
		b = cvScalarI(bUp / (bDown + 0.000001));
		dfb = dfbUp / (dfbDown + 0.000001);
		conf = min(1., cvNorm(f-b) / (dfb + 0.000001)) * exp(-lamda*mFunc(centerColor, f, b));
		alpha = aUp / aDown;
		cvSetReal2D(finalAlpha, unknown->data[i].x, unknown->data[i].y, 
			max(0., min(1., conf * roughAlpha(f, b, centerColor) + (1 - conf) * alpha)));
	}
    releaseGD(&gf);
	if (verbose) {
		long endTime = clock();
		cout << "smoothing time: " << (endTime - startTime + 0.0) / CLOCKS_PER_SEC << endl;
	}
	return finalAlpha;
}

double cvPolyFit1(int x[], int y[], int size, double *paramA = NULL, double *paramB = NULL) {
	double a, b;
	double x2, xx, sumY, sumX, yx, n(size);
	x2 = xx = sumY = sumX = yx = 0;

	for (int i=0; i<n; ++i) {
		x2 += x[i] * x[i];
		yx += y[i] * x[i];
		sumY += y[i];
		sumX += x[i];
		for (int j=0; j<n; ++j) {
			xx += x[i] * x[j];
		}
	}

	a = (yx - sumX*sumY/n) / (x2 - xx/n);
	b = (sumY - a*sumX) / n;

	double ms = 0;
	for (int i=0; i<n; ++i)
		ms += fabs(a*x[i]+b - y[i]);
	//		ms += (a*x[i]+b - y[i])*(a*x[i]+b - y[i]);
	if (paramA != NULL)
		*paramA = a;
	if (paramB != NULL)
		*paramB = b;
	return ms;
}

double calInnerDis(CvMat *src, bool debugOn = false) {
	int size = src->rows * src->cols;
	int *r, *g, *b;
	r = new int[size];
	g = new int[size];
	b = new int[size];

	int cnt=0;
	for (int i=0; i<src->rows; ++i)
		for (int j=0; j<src->cols; ++j) {
			CvScalarI sca = cvScalarI(cvGet2D(src, i, j));
			r[cnt] = sca.data[0];
			g[cnt] = sca.data[1];
			b[cnt] = sca.data[2];
			++cnt;
		}

	double ret = 0;
	ret += cvPolyFit1(r, g, size);
	ret += cvPolyFit1(r, b, size);

	return ret;
}

CvMat* postProcess(CvMat* srcAlpha1, CvMat* srcAlpha2, CvMat* img) {
	CvMat *fImg1;
	fImg1 = cvConvertMatType(cvMul2(img, srcAlpha1), CV_8UC3);
	CvMat *bImg1, *ivSrcAlpha;
	ivSrcAlpha = cvCloneMat(srcAlpha1);
	cvConvertScale(srcAlpha1, ivSrcAlpha, -1, 1);
	bImg1 = cvConvertMatType(cvMul2(img, ivSrcAlpha), CV_8UC3);
	CvMat *fImg2;
	fImg2 = cvConvertMatType(cvMul2(img, srcAlpha2), CV_8UC3);
	CvMat *bImg2;
	ivSrcAlpha = cvCloneMat(srcAlpha2);
	cvConvertScale(srcAlpha2, ivSrcAlpha, -1, 1);
	bImg2 = cvConvertMatType(cvMul2(img, ivSrcAlpha), CV_8UC3);

	CvMat *targetAlpha;
	targetAlpha = cvCloneMat(srcAlpha1);

	cvNamedWindow("tmpMat");

	static const int WD_SIZE_R = 20;
	static const int WD_SIZE_C = 20;
	bool debugOn = false;

	for (int i=0; i<srcAlpha1->rows; i+=WD_SIZE_R)
		for (int j=0; j<srcAlpha1->cols; j+=WD_SIZE_C) {
			cout << i << " " << j << endl;
// 			if (i == 0 && j == 500) {
// 				debugOn = false;
// 			}
			int lx, ly;
			lx = min(WD_SIZE_R, srcAlpha1->rows - i);
			ly = min(WD_SIZE_C, srcAlpha1->cols - j);

// 			//cheat
// 			if (i < 380) {
// 				cvSetSubMat(targetAlpha, i, j, lx, ly, srcAlpha1);
// 				continue;
// 			}

			CvMat *tmpMat;
			tmpMat = cvCopySubMat(fImg1, i, j, lx, ly);
			double inF1 = calInnerDis(tmpMat, debugOn);
			if (debugOn) {
				cvShowImage("tmpMat", tmpMat);
				printMat(tmpMat, "debugTmpmat.txt");
				cvSetMouseCallback("tmpMat", debugByMouseCh, tmpMat);
				cvWaitKey(0);
			}
			cvReleaseMat(&tmpMat);
			tmpMat = cvCopySubMat(bImg1, i, j, lx, ly);
			double inB1 = calInnerDis(tmpMat, debugOn);
			if (debugOn) {
				cvShowImage("tmpMat", tmpMat);
				printMat(tmpMat, "debugTmpmat.txt");
				cvWaitKey(0);
			}
			cvReleaseMat(&tmpMat);
			tmpMat = cvCopySubMat(fImg2, i, j, lx, ly);
			double inF2 = calInnerDis(tmpMat, debugOn);
			if (debugOn) {
				cvShowImage("tmpMat", tmpMat);
				printMat(tmpMat, "debugTmpmat.txt");
				cvSetMouseCallback("tmpMat", debugByMouseCh, tmpMat);
				cvWaitKey(0);
			}
			cvReleaseMat(&tmpMat);
			tmpMat = cvCopySubMat(bImg2, i, j, lx, ly);
			double inB2 = calInnerDis(tmpMat, debugOn);
			if (debugOn) {
				cvShowImage("tmpMat", tmpMat);
				printMat(tmpMat, "debugTmpmat.txt");
				cvWaitKey(0);
			}
			cvReleaseMat(&tmpMat);

			if (debugOn) {
				cout << inF1 << " " << inB1 << " " << inF1 + inB1 << endl
					 << inF2 << " " << inB2 << " " << inF2 + inB2 << endl;
			}
			if (inF1 + inB1 < 1.5*(inF2 + inB2))
				cvSetSubMat(targetAlpha, i, j, lx, ly, srcAlpha1);
			else
				cvSetSubMat(targetAlpha, i, j, lx, ly, srcAlpha2);
		}
	return targetAlpha;
}

double calSAD(CvMat* finalAlpha, CvMat* gt, int unknownPixels, CvMat **diff = (CvMat**)1) {
	double sum = 0;
	bool sav = true;
	if (diff == (CvMat **)1) sav = false;
	else if (*diff == NULL)
		*diff = cvCreateMat(gt->rows, gt->cols, gt->type);
	int val;
	for (int i=0; i<gt->rows; ++i)
		for (int j=0; j<gt->cols; ++j) {
			val = abs(CV_MAT_ELEM(*finalAlpha, uchar, i, j) - CV_MAT_ELEM(*gt, uchar, i, j));
			if (sav) *CV_MAT_ELEM_PTR(**diff, i,j) = val;
			sum += val;
		}
	return sum / 255. / unknownPixels;
}

double calMSE( CvMat* finalAlpha, CvMat* gt ) 
{
	double sum = 0;
	for (int i=0; i<gt->rows; ++i)
		for (int j=0 ;j<gt->cols; ++j)
			sum += pow(double(CV_MAT_ELEM(*finalAlpha, uchar, i, j) - CV_MAT_ELEM(*gt, uchar, i, j)), 2);
	return sum/255/unknownList->length;
}

double calGrad( CvMat* finalAlpha, CvMat* gt ) 
{
	return -1;
	throw std::exception("The method or operation is not implemented.");
}

double calConn( CvMat* finalAlpha, CvMat* gt ) 
{
	return -1;
	throw std::exception("The method or operation is not implemented.");
}

uchar calUniform(uchar x) {
	static uchar uniTable[256];
	static bool caled = false;
	if (caled) return uniTable[x];

	int firstNon = -1;
	int cnt = 0;
	for (int i=0; i<256; ++i) {
		int tmp = i;
		int turn = 0;
		int state = tmp%2;
		for (int j=0; j<8; ++j) {
			int r = tmp%2;
			if (r != state) {
				state = r;
				++turn;
			}
			tmp = tmp >> 1;
		}
		if (turn<=2) {
			uniTable[i] = i;
			++cnt;
		} else {
			if (firstNon == -1)
				firstNon = i;
			uniTable[i] = firstNon;
		}
	}

	caled = true;
	return uniTable[x];
}

CvMat* calLBP(CvMat* src, bool useUniform = false) {
    int incx[] = {1,1, 1, 0,-1,-1,-1,0};
    int incy[] = {1,0,-1,-1,-1, 0, 1,1};
    CvMat* grayImg = src;
    CvMat* lbpImg = cvCreateMat(grayImg->rows, grayImg->cols, CV_8U);
    for (int i=0; i<grayImg->rows; ++i) {
        for (int j=0; j<grayImg->cols; ++j) {
            double c = cvGetReal2D(grayImg, i, j);
            uchar val = 0;
            for (int k=0; k<8; ++k) {
                int a = incx[k]+i;
                int b = incy[k]+j;
                val = val << 1;
                if (0<=a && a<grayImg->rows && 0<=b && b<grayImg->cols) {
                    if (c > cvGetReal2D(grayImg, a, b))
                        val += 1;
                }
            }
			if (useUniform)
				*CV_MAT_ELEM_PTR(*lbpImg, i, j) = calUniform(val);
			else
				*CV_MAT_ELEM_PTR(*lbpImg, i, j) = val;
        }
    }
    return lbpImg;
}

const int FEATURE_DIM_X = 5;
const int FEATURE_DIM_Y = 5;
const int FEATURE_DIMS[] = {FEATURE_DIM_X, FEATURE_DIM_Y};
const int FEATURE_SIZE = 16;
typedef struct TFeature {
    uchar val[FEATURE_SIZE];
} TFeature;

TFeature **features;

void debugByMouseLBP(int type, int x, int y, int flag, void* param) {
	switch (type) {
		case CV_EVENT_LBUTTONDOWN:
			CvMat* mat = (CvMat*) param;
			printf("point at (%d, %d) = %d\n", y, x, CV_MAT_ELEM(*imgMaskGray, uchar, y, x));
			printf("feature: ");
			for (int i=0; i<FEATURE_SIZE; ++i)
				printf("%d ", features[y][x].val[i]);
			printf("\n");
			break;
	}
}

TFeature **getFeatures(CvMat* src) {
	long startTime = clock();
    TFeature **features = new TFeature*[src->rows];
    int cnt = 0;
    double data = 0;

    for (int i=0; i<src->rows; ++i) {
        features[i] = new TFeature[src->cols];
        for (int j=0; j<src->cols; ++j) {
            memset(features[i][j].val, 0, FEATURE_SIZE);
            for (int u=max(0, i-2); u<min(src->rows, i+3); ++u)
                for (int v=max(0, j-2); v<min(src->cols, j+3); ++v) {
                    data = cvGetReal2D(src, u, v);
                    //++features[i][j].val[int((data+254)/(510/FEATURE_SIZE))];
					++features[i][j].val[int(data/(256/FEATURE_SIZE))];
                }
        }
    }
	long endTime = clock();
	cout << "Get Feature time: " << (endTime - startTime+0.0) / CLOCKS_PER_SEC << endl;

    return features;
}


void writeProb2File(char filename[]) {
	FILE* outF;
	fopen_s(&outF, filename, "w");
	int unCnt = unknownList->length;
	int knCnt = img->rows*img->cols - unCnt;
	int wrtKnCnt = knCnt / 20;
	fprintf_s(outF, "%d %d\n", wrtKnCnt, FEATURE_SIZE);
	bool *mark;
	mark = new bool[knCnt];
	memset(mark, 0, knCnt);
	int tmp = wrtKnCnt;
	int fCnt = 0, bCnt = 0;
	while (tmp > 0) {
		int choose = rand()*rand()%knCnt;
		if (mark[choose] == false) {
			mark[choose] = true;
			--tmp;
		}
	}
	tmp = 0;
	for (int i=0; i<img->rows; ++i)
		for (int j=0; j<img->cols; ++j) {
			uchar mv = CV_MAT_ELEM(*imgMaskGray, uchar, i, j);
			if (mv == 127) continue;
			if (!mark[tmp++]) continue;
			fprintf_s(outF, "%d\t", mv>0);
			for (int k=0; k<FEATURE_SIZE; ++k)
				fprintf_s(outF, "%d:%d ", k+1, features[i][j].val[k]);
			fprintf_s(outF, "\n");
		}	

	delete[] mark;
	fclose(outF);
}

CvMat *useSvmTrain(void) {
	char trFile[] = "svmTrain.txt";
	char modelFile[] = "svmModel";
	char predFile[] = "svmPredict.txt";
	char tsFile[] = "svmTest.txt";

	writeProb2File(trFile);

	UsingSVM usingSVM;
	svm_model *model;

	usingSVM.read_problem(trFile);
	usingSVM.initParam();

	cout << "SVM setting finished, start training.." << endl;

	long startTime, endTime;
	startTime = clock();
	model = usingSVM.train();
	endTime = clock();
	cout << "training time: " << (endTime - startTime + 0.0) / CLOCKS_PER_SEC << endl;

	if(svm_save_model(modelFile, model)) {
		cout << "can't save model to file" << endl;
		exit(1);
	}

	cout << "##################start predict!#####################" << endl;
	startTime = clock();
	
	struct svm_node testVec[FEATURE_SIZE+1];
	int n = unknownList->length;

	CvMat *svmRes = cvCreateMat(img->rows, img->cols, CV_32F);
	for (int i=0; i<img->rows; ++i)
		for (int j=0; j<img->cols; ++j) {
			uchar mv = CV_MAT_ELEM(*imgMaskGray, uchar, i, j);
			if (mv == 0) {
				cvSetReal2D(svmRes, i, j, 0);
				continue;
			}
			if (mv == 255) {
				cvSetReal2D(svmRes, i, j, 1);
				continue;
			}
			double alpha;
			for (int k=0; k<FEATURE_SIZE; ++k) {
				testVec[k].index = k+1;
				testVec[k].value = features[i][j].val[k];
			}
			testVec[FEATURE_SIZE].index = -1;
			svm_predict_values(model, testVec, &alpha);
			cvSetReal2D(svmRes, i, j, 1-alpha);
		}

	usingSVM.clear();

	endTime = clock();
	cout << "predict time: " << (endTime - startTime + 0.0) / CLOCKS_PER_SEC << endl;
	return svmRes;
}

void newMethods_filter(void) {
	cvNamedWindow("input", CV_WINDOW_AUTOSIZE);
	cvShowImage("input", img);
	
	cvNamedWindow("inputMask");
	cvShowImage("inputMask", imgMaskGray);
	cvNamedWindow("inputGT");
	cvShowImage("inputGT", gt);
	cvNamedWindow("lbp");
	cvShowImage("lbp", calLBP(rgb2gray(img)));

	CvMat* grad = cvCloneMat(img);
	cvSobel(img, grad, 1, 1);
	
	int cnt[256];
	memset(cnt, 0, 4*256);

	CvMat* targ = cvCreateMat(img->rows, img->cols, CV_8U);
	for (int i=0; i<img->rows; ++i)
		for (int j=0; j<img->cols; ++j) {
			CvScalarI sca = cvScalarI(cvGet2D(grad, i, j));
			for (int k=0; k<3; ++k)
				++cnt[sca.data[k]];
		}
	int total = img->rows * img->cols * 3;
	int keep = total / 20;
	int lowStart = 0;

	for (int i=255; i>=0; --i)
		if (cnt[i] != 0) {
			cout << i << " " << cnt[i] << endl;
			keep -= cnt[i];
			if (keep <= 0) {
				lowStart = i;
				break;
			}
		}

	for (int i=0; i<img->rows; ++i)
		for (int j=0; j<img->cols; ++j) {
			CvScalarI sca = cvScalarI(cvGet2D(grad, i ,j));
			cvSetReal2D(targ, i, j, 0);
			for (int k=0; k<3; ++k)
				if (sca.data[k] >= lowStart) {
					cvSetReal2D(targ, i, j, 255);
					break;
				}
		}
	cvNamedWindow("targ");
	cvShowImage("targ", targ);
// 	printMat(grad, "debugGrad.txt");
// 	CvMat* grad2 = cvCloneMat(img);
// 	cvSobel(grad, grad2, 1, 1);
// 	cvNamedWindow("lap");
// 	cvShowImage("lap", grad2);
// 	printMat(grad2, "debugGrad2.txt");

	cvWaitKey(0);
}

void newMethods_diff(void) {
	cvNamedWindow("input", CV_WINDOW_AUTOSIZE);
	cvShowImage("input", img);

	cvNamedWindow("inputMask");
	cvShowImage("inputMask", imgMaskGray);
	cvNamedWindow("inputGT");
	cvShowImage("inputGT", gt);

// 	CvMat *gray = rgb2gray(img);
// 	CvMat* smooth = cvCloneMat(gray);
// 	cvSmooth(gray, smooth, CV_BILATERAL);
// 	CvMat* res = cvCloneMat(gray);
// 	cvAbsDiff(gray, smooth, res);
// 
// 
	CvMat *hsv = cvCreateMat(img->rows, img->cols, CV_32FC3);
	cvCvtColor(cvConvertMatType(img, CV_32FC3), hsv, CV_BGR2HSV);
	CvMat *h, *s, *v;
	h = cvCreateMat(img->rows, img->cols, CV_32F);
	s = cvCloneMat(h);
	v = cvCloneMat(h);
	cvSplit(hsv, h, s, v, NULL);
// 	cvNamedWindow("s");
// 	cvShowImage("s", s);
	cvNamedWindow("h");
	cvScale(h, h, 1/360.0);
	cvShowImage("h", h);
// 
// 
    cvScale(h, h, 255);
    h = cvConvertMatType(h, CV_8U);
    cvScale(s, s, 255);
    s = cvConvertMatType(s, CV_8U);
    v = cvConvertMatType(v, CV_8U);
	cvSaveMatImage("h.bmp", h);
	cvSaveMatImage("v.bmp", v);
	cvSaveMatImage("s.bmp", s);
    CvMat *targ = cvCreateMat(img->rows, img->cols, CV_8UC3);
    cvMerge(v, s, h, NULL, targ);
    cvShowImage("debug", targ);
	cvWaitKey(0);
	exit(0);

	CvMat* grad = cvCloneMat(img);
	cvSobel(img, grad, 1, 1);

	CvMat* grad2 = cvCreateMat(img->rows, img->cols, CV_16SC3);
	grad = h;

	int **im;
	im = new int*[grad->rows];
	for (int i=0; i<grad->rows; ++i) {
		im[i] = new int[grad->cols];
		for (int j=0; j<grad->cols; ++j)
			im[i][j] = i*grad->cols + j;
	}

	int **heights;
	heights = new int*[grad->rows];
	for (int i=0; i<grad->rows; ++i) {
		heights[i] = new int[grad->cols];
		memset(heights[i], 0, 4*grad->cols);
	}

	const int WIN_SIZE = 3;
	int cn = CV_MAT_CN(grad->type);

	for (int i=0; i<grad->rows; ++i)
		for (int j=0; j<grad->cols; ++j) {
			//deal rows
			CvScalar sca = cvScalar(0);
			int cnt = 0;
			for (int k=max(0, i-WIN_SIZE); k<=min(grad->rows-1, i+WIN_SIZE); ++k) {
				sca = sca + cvGet2D(grad, k, j);
				++cnt;
			}
			CvScalar avgSca = sca / cnt;
			cnt = 0;
			double maxVal = -1;
			int maxIdx = -1;
			for (int k=max(0, i-WIN_SIZE); k<=min(grad->rows-1, i+WIN_SIZE); ++k) {
				sca = cvGet2D(grad, k, j);
				double val = cvNorm(avgSca - sca);
				if (val > maxVal) {
					maxVal = val;
					maxIdx = k;
				}
// 				int vote = 0;
// 				for (int t=0; t<cn; ++t)
// 					if (sca.val[t] > avgSca.val[t])
// 						++vote;
// 				type[cnt++] = (vote>cn/2)?1:0;
// 				++cntType[type[cnt-1]];
			}
			++heights[maxIdx][j];

// 			for (int k=max(0, i-WIN_SIZE); k<=min(grad->rows-1, i+WIN_SIZE); ++k) 
// 				if (type[k-max(0, i-WIN_SIZE)] == (cntType[0] > cntType[1]))
// 					++heights[k][j];


			//deal columns
			sca = cvScalar(0);
			cnt = 0;
			for (int k=max(0, j-WIN_SIZE); k<=min(grad->cols-1, j+WIN_SIZE); ++k) {
				sca = sca + cvGet2D(grad, i, k);
				++cnt;
			}
			avgSca = sca / cnt;
			
			cnt = 0;
			maxVal = -1;
			for (int k=max(0, j-WIN_SIZE); k<=min(grad->cols-1, j+WIN_SIZE); ++k) {
				sca = cvGet2D(grad, i, k);
				double val = cvNorm(avgSca - sca);
				if (maxVal < val) {
					maxVal = val;
					maxIdx = k;
				}
// 				int vote = 0;
// 				for (int t=0; t<cn; ++t)
// 					if (sca.val[t] > avgSca.val[t])
// 						++vote;
// 				type[cnt++] = (vote>cn/2)?1:0;
// 				++cntType[type[cnt-1]];
			}
			++heights[i][maxIdx];

// 			for (int k=max(0, j-WIN_SIZE); k<=min(grad->cols-1, j+WIN_SIZE); ++k)
// 				if (type[k-max(0, j-WIN_SIZE)] == (cntType[0] > cntType[1]))
// 					++heights[i][k];
		}

	CvMat *heightMat = cvCreateMat(img->rows, img->cols, CV_8U);
	for (int i=0; i<img->rows; ++i)
		for (int j=0; j<img->cols; ++j) {
// 			if (CV_MAT_ELEM(*imgMaskGray, uchar, i, j) < 127)
// 				heights[i][j] = 0;
			cvSetReal2D(heightMat, i, j, heights[i][j]*10);
		}
	cvNamedWindow("height");
	cvShowImage("height", heightMat);
//  	printMat(heightMat, "debugHeight.txt");
	cvSaveMatImage("h.bmp", heightMat);

	cvWaitKey(0);
}

void newMethods_SVM(void) {
	CvMat *hCh = cvCreateMat(img->rows, img->cols, CV_8U);
	cvSplit(img, hCh, NULL, NULL, NULL);

	CvMat *lbp = calLBP(getCn(img, 2), true);
	features = getFeatures(lbp);

	cvNamedWindow("lbp");
	cvShowImage("lbp", lbp);
	cvSetMouseCallback("lbp", debugByMouseLBP, lbp);

	CvMat *svmRes = useSvmTrain();
	cvNamedWindow("svm");
	cvShowImage("svm", svmRes);
	printMat(svmRes, "debugSVM.txt");
	cvSaveMatImage("svmRes.bmp", svmRes);
}

void newMethods_fit(void) {
// 	CvMat *sub = cvCopySubMat(img, 0, 0, 10, 10);
// 	calInnerDis(sub);

// 	pointList[0].x = 0;
// 	pointList[0].y = 1;
// 	pointList[1].x = 2;
// 	pointList[1].y = -1;
	int x[] = {1, 2, 4};
	int y[] = {0, 0, 1};
	double a, b;
	cout << cvPolyFit1(x, y, 3, &a, &b) << endl;
	cout << a << " " << b << endl;
}

void patchTest(void) {
	char testFile[] = "GT";
	char tmpName[100];
	SYSTEM_INFO sInfo;
	GetSystemInfo(&sInfo);
	pn = sInfo.dwNumberOfProcessors*2;

	long totStart = clock();
	double sumSAD = 0;

	for (int testI = 1; testI <= 27; ++testI) {
		long startTime = clock();

		sprintf_s(tmpName, 100, "input\\%s%02d.png\0", testFile, testI);
		img = imread(tmpName);
		sprintf_s(tmpName, 100, "trimap\\%s%02d.png\0", testFile, testI);
		imgMask = imread(tmpName);
		imgMaskGray = RefineMask(rgb2gray(imgMask));
		sprintf_s(tmpName, 100, "answer\\%s%02d.png\0", testFile, testI);
		gt = rgb2gray(imread(tmpName));

		CvMat *finalAlpha = calAlpha();

		CvMat* res;
		res = cvMul2(img, finalAlpha);
		CvMat* charRes;
		charRes = cvCreateMat(res->rows, res->cols, CV_MAKETYPE(CV_8U, 3));
		cvConvert(res, charRes);

		long endTime = clock();
		cout << testI
			<< "total time: " << (endTime - startTime + 0.0) / CLOCKS_PER_SEC << endl;

		CvMat* charFinalAlpha;
		charFinalAlpha = cvMul2(255, finalAlpha);
		double sad = calSAD(charFinalAlpha, gt, unknownPixels);
		sumSAD += sad;
		cout << "error:" << endl
			<< "\tSAD: " << sad << endl
			<< "\tMSE: " << calMSE(charFinalAlpha, gt) << endl;
	//		<< "\tGradient: " << calGrad(finalAlpha, gt) << endl
	//		<< "\tConnectivity: " << calConn(finalAlpha, gt) << endl;

		for (int i=0; i<imgMaskGray->rows; ++i)
			delete[] un2PosMap[i];
		delete[] un2PosMap;
		delete[] unknownList->data;
		delete[] unsampled.data;
		for (int rowStart=0; rowStart<SPLIT+1; ++rowStart)
			for (int colStart=0; colStart<SPLIT+1; ++colStart) {
				int idx = (rowStart<<SPLIT) + colStart;
				delete[] fgPoint[idx].data;
				delete[] bgPoint[idx].data;
			}
		cvReleaseMat(&finalAlpha);
		cvReleaseMat(&res);
		cvReleaseMat(&charRes);
		cvReleaseMat(&charFinalAlpha);
		cvReleaseMat(&img);
		cvReleaseMat(&imgMask);
		cvReleaseMat(&imgMaskGray);
		cvReleaseMat(&gt);
	}

	long totEnd = clock();
	cout << "all test time: " << (totEnd - totStart + 0.0) / CLOCKS_PER_SEC << endl;
	cout << "sum of SAD: " << sumSAD << endl
		<< "average SAD: " << sumSAD / 27 << endl;
}

inline int cvGetMatType(CvMat *src) {return CV_MAT_DEPTH(src->type);}

template <typename T>
T **cvNewArray2(int rows, int cols, T) {
    T **ret;
    ret = new T*[rows];
    for (int i=0; i<rows; ++i) {
        ret[i] = new T[cols];
        memset(ret[i], 0, sizeof(T) * cols);
    }
    return ret;
}

template <typename T>
void cvDeleteArray2(T **&src, int rows) {
    for (int i=0; i<rows; ++i)
        delete[] src[i];
    delete src;
    src = 0;
}

//only for single channel mat
template <typename T>
T **cvMat2Arr(CvMat *src, T *data) {
	T **res;
	res = new T *[src->rows];
	for (int i=0; i<src->rows; ++i)
		res[i] = data + i * src->cols;
	return res;
}

//only for single channel mat
CvMat* cvBoxFilter(CvMat *src, int r) {
    CvMat *res = cvCreateMat(src->rows, src->cols, CV_32F);
    float **sum = cvNewArray2(src->rows, src->cols, float(0));
	float **fl = NULL;
	uchar **ptr = NULL;
	bool isUchar = true;
	if (cvGetMatType(src) == CV_32F) {
		isUchar = false;
		fl = cvMat2Arr(src, src->data.fl);
		for (int i=0; i<src->rows; ++i) {
			sum[i][0] = fl[i][0];
			for (int j=1; j<src->cols; ++j)
				sum[i][j] = fl[i][j] + sum[i][j-1];
		}
		delete[] fl;
	}
	else {
		ptr = cvMat2Arr(src, src->data.ptr);
		for (int i=0; i<src->rows; ++i) {
			sum[i][0] = ptr[i][0];
			for (int j=1; j<src->cols; ++j)
				sum[i][j] = ptr[i][j] + sum[i][j-1];
		}
		delete[] ptr;
	}

    float **tmp = cvNewArray2(src->rows, src->cols, float(0));
    for (int i=0; i<src->rows; ++i) {
        for (int j=0; j<r+1; ++j)
            tmp[i][j] = sum[i][j+r];
        for (int j=r+1; j<src->cols-r-1; ++j)
            tmp[i][j] = sum[i][j+r] - sum[i][j-r-1];
        for (int j=src->cols-r-1; j<src->cols; ++j)
            tmp[i][j] = sum[i][src->cols-1] - sum[i][j-r-1];
    }

    for (int j=0; j<src->cols; ++j) {
        sum[0][j] = tmp[0][j];
        for (int i=1; i<src->rows; ++i) {
            sum[i][j] = tmp[i][j] + sum[i-1][j];
        }
    }
    for (int j=0; j<src->cols; ++j) {
        for (int i=0; i<r+1; ++i)
            tmp[i][j] = sum[i+r][j];
        for (int i=r+1; i<src->rows-r-1; ++i)
            tmp[i][j] = sum[i+r][j] - sum[i-r-1][j];
        for (int i=src->rows-r-1; i<src->rows; ++i)
            tmp[i][j] = sum[src->rows-1][j] - sum[i-r-1][j];
    }

	float *resFl = res->data.fl;
    for (int i=0; i<src->rows; ++i)
		for (int j=0; j<src->cols; ++j, ++resFl)
			*resFl = tmp[i][j];
    cvDeleteArray2(sum, src->rows);
    cvDeleteArray2(tmp, src->rows);
    return res;
}

CvMat *cvBoxFilter(CvMat *src, int r, CvMat *normalizer) {
    CvMat *res = cvBoxFilter(src, r);
    cvDiv(res, normalizer, res);
    return res;
}

CvMat *guidedFilter(CvMat *I, CvMat *inP, int r, double eps) {
    CvMat *ones = cvCreateMat(I->rows, I->cols, CV_8U);
    cvSet(ones, cvScalar(1));
    CvMat *N = cvBoxFilter(ones, r);

    CvMat *channelR, *channelG, *channelB;
    channelR = cvCreateMat(I->rows, I->cols, cvGetMatType(I));
    channelG = cvCloneMat(channelR);
    channelB = cvCloneMat(channelR);
    cvSplit(I, channelR, channelG, channelB, NULL);
    CvMat *meanIR, *meanIG, *meanIB;
    meanIR = cvBoxFilter(channelR, r, N);
    meanIG = cvBoxFilter(channelG, r, N);
    meanIB = cvBoxFilter(channelB, r, N);
	CvMat *buf;
	buf = cvConvertMatType(channelR, CV_32F);
	cvReleaseMat(&channelR);
	channelR = buf;
	buf = cvConvertMatType(channelG, CV_32F);
	cvReleaseMat(&channelG);
	channelG = buf;
	buf = cvConvertMatType(channelB, CV_32F);
	cvReleaseMat(&channelB);
	channelB = buf;
	CvMat *p = cvConvertMatType(inP, CV_32F);

    CvMat *meanP;
    meanP = cvBoxFilter(p, r, N);

    buf = cvCloneMat(meanP);

    CvMat *meanIRP, *meanIGP, *meanIBP;
    cvMul(channelR, p, buf);
    meanIRP = cvBoxFilter(buf, r, N);
    cvMul(channelG, p, buf);
    meanIGP = cvBoxFilter(buf, r, N);
    cvMul(channelB, p, buf);
    meanIBP = cvBoxFilter(buf, r, N);

    CvMat *covIRP, *covIGP, *covIBP;
    covIRP = cvCloneMat(meanIRP);
    cvMul(meanIR, meanP, buf);
    cvSub(meanIRP, buf, covIRP);
    covIGP = cvCloneMat(meanIRP);
    cvMul(meanIG, meanP, buf);
    cvSub(meanIGP, buf, covIGP);
    covIBP = cvCloneMat(meanIRP);
    cvMul(meanIB, meanP, buf);
    cvSub(meanIBP, buf, covIBP);

    CvMat *varIRR, *varIRG, *varIRB, *varIGG, *varIGB, *varIBB;
    CvMat *buf2;
    cvMul(channelR, channelR, buf);
    buf2 = cvBoxFilter(buf, r, N);
    cvMul(meanIR, meanIR, buf);
    varIRR = cvCloneMat(meanIR);
    cvSub(buf2, buf, varIRR);
    cvReleaseMat(&buf2);
    cvMul(channelR, channelG, buf);
    buf2 = cvBoxFilter(buf, r, N);
    cvMul(meanIR, meanIG, buf);
    varIRG = cvCloneMat(meanIR);
    cvSub(buf2, buf, varIRG);
    cvReleaseMat(&buf2);
    cvMul(channelR, channelB, buf);
    buf2 = cvBoxFilter(buf, r, N);
    cvMul(meanIR, meanIB, buf);
    varIRB = cvCloneMat(meanIR);
    cvSub(buf2, buf, varIRB);
    cvReleaseMat(&buf2);
    cvMul(channelG, channelG, buf);
    buf2 = cvBoxFilter(buf, r, N);
    cvMul(meanIG, meanIG, buf);
    varIGG = cvCloneMat(meanIR);
    cvSub(buf2, buf, varIGG);
    cvReleaseMat(&buf2);
    cvMul(channelG, channelB, buf);
    buf2 = cvBoxFilter(buf, r, N);
    cvMul(meanIG, meanIB, buf);
    varIGB = cvCloneMat(meanIR);
    cvSub(buf2, buf, varIGB);
    cvReleaseMat(&buf2);
    cvMul(channelB, channelB, buf);
    buf2 = cvBoxFilter(buf, r, N);
    cvMul(meanIB, meanIB, buf);
    varIBB = cvCloneMat(meanIR);
    cvSub(buf2, buf, varIBB);
    cvReleaseMat(&buf2);

    float **varMat[3][3];
    varMat[0][0] = cvMat2Arr(varIRR, varIRR->data.fl);
    varMat[0][1] = cvMat2Arr(varIRG, varIRG->data.fl);
    varMat[0][2] = cvMat2Arr(varIRB, varIRB->data.fl);
    varMat[1][1] = cvMat2Arr(varIGG, varIGG->data.fl);
    varMat[1][2] = cvMat2Arr(varIGB, varIGB->data.fl);
    varMat[2][2] = cvMat2Arr(varIBB, varIBB->data.fl);
    float **covIPMat[3];
    covIPMat[0] = cvMat2Arr(covIRP, covIRP->data.fl);
    covIPMat[1] = cvMat2Arr(covIGP, covIGP->data.fl);
    covIPMat[2] = cvMat2Arr(covIBP, covIBP->data.fl);

    CvMat *a[3];
    float **flA[3];
    for (int i=0; i<3; ++i) {
        a[i] = cvCreateMat(I->rows, I->cols, CV_32F);
        flA[i] = cvMat2Arr(a[i], a[i]->data.fl);
    }
    CvMat *sigma = cvCreateMat(3, 3, CV_32F);
    CvMat *invSigma = cvCloneMat(sigma);
    CvMat *epsEye = cvCloneMat(sigma);
    CvMat *mulResA = cvCreateMat(1, 3, CV_32F);
    CvMat *covIP = cvCloneMat(mulResA);
    cvSetIdentity(epsEye);
    cvScale(epsEye, epsEye, eps);
    float **flSigma = cvMat2Arr(sigma, sigma->data.fl);
    for (int i=0; i<I->rows; ++i) {
        for (int j=0; j<I->cols; ++j) {
            for (int u=0; u<3; ++u) {
                for (int v=u; v<3; ++v) {
                    flSigma[u][v] = varMat[u][v][i][j];
                    if (u == v) continue;
                    flSigma[v][u] = flSigma[u][v];
                }
            }
            cvAdd(sigma, epsEye, sigma);
            cvInvert(sigma, invSigma);
            for (int k=0; k<3; ++k)
                covIP->data.fl[k] = covIPMat[k][i][j];
            cvMatMul(covIP, invSigma, mulResA);
            for (int k=0; k<3; ++k)
                flA[k][i][j] = mulResA->data.fl[k];
        }
    }

    CvMat *mulBuf;
    mulBuf = cvCloneMat(meanP);
    cvMul(a[0], meanIR, mulBuf);
    cvSub(meanP, mulBuf, buf);
    cvMul(a[1], meanIG, mulBuf);
    cvSub(buf, mulBuf, buf);
    cvMul(a[2], meanIB, mulBuf);
    cvSub(buf, mulBuf, buf);
    CvMat *b = cvCloneMat(buf);

    CvMat *q = cvBoxFilter(b, r, N);
    cvReleaseMat(&buf);
    buf = cvBoxFilter(a[0], r, N);
    cvMul(buf, channelR, mulBuf);
    cvAdd(q, mulBuf, q);
    cvReleaseMat(&buf);
    buf = cvBoxFilter(a[1], r, N);
    cvMul(buf, channelG, mulBuf);
    cvAdd(q, mulBuf, q);
    cvReleaseMat(&buf);
    buf = cvBoxFilter(a[2], r, N);
    cvMul(buf, channelB, mulBuf);
    cvAdd(q, mulBuf, q);

    cvReleaseMat(&meanP);
    cvReleaseMat(&buf);
    cvReleaseMat(&meanIRP);
    cvReleaseMat(&meanIGP);
    cvReleaseMat(&meanIBP);
    cvReleaseMat(&covIRP);
    cvReleaseMat(&covIGP);
    cvReleaseMat(&covIBP);
    cvReleaseMat(&varIRR);
    cvReleaseMat(&varIRG);
    cvReleaseMat(&varIRB);
    cvReleaseMat(&varIGG);
    cvReleaseMat(&varIGB);
    cvReleaseMat(&varIBB);
    for (int i=0; i<3; ++i)
        for (int j=i; j<3; ++j)
            delete[] varMat[i][j];
    for (int i=0; i<3; ++i)
        delete[] covIPMat[i];
    for (int i=0; i<3; ++i) {
        cvReleaseMat(&a[i]);
        delete[] flA[i];
    }
    cvReleaseMat(&sigma);
	cvReleaseMat(&invSigma);
	cvReleaseMat(&epsEye);
	cvReleaseMat(&mulResA);
	cvReleaseMat(&covIP);
	delete[] flSigma;
	cvReleaseMat(&mulBuf);
	cvReleaseMat(&b);

    return q;
}

template <typename T>
void cvLimit(CvMat *src, T *data, float low, float up) {
	T **ptr = cvMat2Arr(src, data);
	for (int i=0; i<src->rows; ++i)
		for (int j=0; j<src->cols; ++j)
			ptr[i][j] = min(up, max(low, ptr[i][j]));
	delete[] ptr;
}

void newMethods_guided(void) {
	CvMat *finalAlpha = calAlpha();//cvConvertMatType(imgMaskGray, CV_32F);
	cvNamedWindow("img");
	cvNamedWindow("finalAlpha");
	cvNamedWindow("guided");
	CvMat *guided= guidedFilter(img, finalAlpha, 10, 0.01);
	cvLimit(guided, guided->data.fl, 0, 1);
	cvShowImage("img", img);
	cvShowImage("finalAlpha", finalAlpha);
	cvShowImage("guided", guided);
	cvNamedWindow("diff");
	CvMat *charGuided = cvMul2(255, guided);
	printMat(guided, "debugGuided.txt");
	printMat(charGuided, "debugCharGuided.txt");
	
	CvMat *diff = NULL;
	cout << calSAD(charGuided, gt, unknownPixels, &diff) << endl;
	cvNamedWindow("diff");
	cvShowImage("diff", diff);
	cvWaitKey(0);
}

void newMethods(void) {
// 	CvMat *post = loadMat("debugPost.txt");
// 	cvNamedWindow("post");
// 	cvShowImage("post", post);
	CvMat *hsvAlpha = loadMat("debugHsvFa.txt");
	CvMat *normalAlpha = loadMat("debugFa.txt");
	CvMat *post = postProcess(normalAlpha, hsvAlpha, img);

	cvNamedWindow("hsvAlpha");
	cvNamedWindow("normalAlpha");
	cvNamedWindow("post");
	cvShowImage("hsvAlpha", hsvAlpha);
	cvShowImage("normalAlpha", normalAlpha);
	cvShowImage("post", post);

	CvMat *guided = guidedFilter(img, post, 5, 0.001);
	cvLimit(guided, guided->data.fl, 0, 1);
	cvNamedWindow("guided");
	cvShowImage("guided", guided);
	cvSaveMatImage("guidedPost.png", guided);

	CvMat *charGuided = cvMul2(255, guided);
	CvMat *diff = NULL;
	cout << calSAD(charGuided, gt, unknownPixels, &diff) << endl;
	cvNamedWindow("diff");
	cout << diff->rows << " " << diff->cols << endl;
	cvShowImage("diff", diff);
	cvWaitKey(0);
}

void newMethods_mix(void) {
	char testFile[] = "GT04";
	char tmpName[100];

	long startTime = clock();

	CvMat *hsvImg, *hCh;
	hsvImg = cvConvertMatType(img, CV_32FC3);
	cvCvtColor(hsvImg, hsvImg, CV_BGR2HSV);
	hCh = cvConvertMatType(getCn(hsvImg, 0), CV_8U);
	cvMerge(hCh, hCh, hCh, NULL, img);
	CvMat *hsvAlpha = calAlpha();

	printMat(hsvAlpha, "debugHsvFa.txt");

	sprintf_s(tmpName, 100, "%s.png\0", testFile);
	img = imread(tmpName);
	sprintf_s(tmpName, 100, "%s_m.png\0", testFile);
	imgMask = imread(tmpName);
	imgMaskGray = RefineMask(rgb2gray(imgMask));
	sprintf_s(tmpName, 100, "%s_T.png\0", testFile);
	gt = rgb2gray(imread(tmpName));

	CvMat *finalAlpha = calAlpha();

//	printMat(finalAlpha, "debugFa.txt");

	cvNamedWindow("hsvAlpha");
	cvShowImage("hsvAlpha", hsvAlpha);


	CvMat* postProImg;
	postProImg = postProcess(finalAlpha, hsvAlpha, img);
	// 	postProImg = finalAlpha;

//	printMat(postProImg, "debugPost.txt");

	cvNamedWindow("postAlpha");
	cvShowImage("postAlpha", postProImg);

//	printMat(postProImg, "debugPost.txt");
	CvMat *guided = guidedFilter(img, postProImg, 20, 0.001);
	printMat(guided, "debugGuided.txt");
	cvLimit(guided, guided->data.fl, 0, 1);
	cvNamedWindow("guided");
	cvShowImage("guided", guided);

	finalAlpha = postProImg;

	CvMat* res;
	res = cvMul2(img, finalAlpha);
	CvMat* charRes;
	charRes = cvCreateMat(res->rows, res->cols, CV_MAKETYPE(CV_8U, 3));
	cvConvert(res, charRes);

	cvShowImage("debug", charRes);

	long endTime = clock();
	cout << "total time: " << (endTime - startTime + 0.0) / CLOCKS_PER_SEC << endl;

	CvMat* charFinalAlpha;
	charFinalAlpha = cvMul2(255, finalAlpha);
	CvMat *diff = NULL;
	cout << "error:" << endl
		<< "\tSAD: " << calSAD(charFinalAlpha, gt, unknownPixels, &diff) << endl
		<< "\tMSE: " << calMSE(charFinalAlpha, gt) << endl;
	//		<< "\tGradient: " << calGrad(finalAlpha, gt) << endl
	//		<< "\tConnectivity: " << calConn(finalAlpha, gt) << endl;
	cvNamedWindow("diff");
	cvShowImage("diff", diff);
	cvWaitKey(0);
	cvDestroyAllWindows();

}

class Matting
{
private:
	Mat RGB2Gray(Mat& img)
	{
	}

	Mat RefineMask(Mat& inMask)
	{
	}

	int AnalyseTrimap(Mat& mask, vector<Point>& fg, vector<Point>& bg, vector<Point>& unknown, int** un2pos = NULL) {
		fg.clear();
		bg.clear();
		unknown.clear();

		for (int i = 0; i < mask.rows; ++i) {
			uchar *p = mask.row(i).ptr();
			for (int j = 0; j < mask.cols; ++j, ++p)
				if (*p == 127) {
					if (un2pos != NULL)
						un2pos[i][j] = unknown.size();
					unknown.push_back(Point(i, j));
				}
				else if (*p == 0)
					bg.push_back(Point(i, j));
				else if (*p == 255)
					fg.push_back(Point(i, j));
		}

		return 0;
	}

	Point TinyDiff(Mat& img, Mat& imgMask, const Point& center, int cDif, int dDif) {
		int sum = 0;
		Scalar_<int> color;
		Scalar_<int> centerColor;
		int mincDif = cDif + 1;
		Point res(-1, -1);
		uchar* maskPtr;

		centerColor = img.at
		cvScalarI(cvPtr2D(img, center.x, center.y));
		for (int i=max(0, center.x-dDif); i<min(img->rows, center.x+dDif); ++i) {
			color = cvScalarI(cvPtr2D(img, i, max(0, center.y-dDif)));
			maskPtr = cvPtr2D(imgMask, i, max(0, center.y-dDif));
			for (int j=max(0, center.y-dDif); j<min(img->cols, center.y+dDif); ++j, ++maskPtr, ++color)
				if (*maskPtr != 127) {
					sum = cvSum3(cvAbs3(color - centerColor));
					if (sum < mincDif) {
						mincDif = sum;
						res->x = i;
						res->y = j;
					}
					if (mincDif == 0)
						return res;
				}
		}
		if (res->x == -1)
			return NULL;
		else
			return res;
	}

	typedef struct ExpansionPara {
		CvMat* img;
		CvMat* imgMask;
		int cDif;
		int dDif;
		CvMat* tmp;
		CvPointList* unknown;
		int start;
		int end;
	} ExpansionPara;

	void ExpansionK(ExpansionPara* param) {
		for (int i=param->start; i<param->end; ++i) {
			CvPoint* sample = tinyDif(param->img, param->tmp, param->unknown->data[i], param->cDif, param->dDif);
			if (sample != NULL)
				CV_MAT_ELEM(*param->imgMask, uchar, param->unknown->data[i].x, param->unknown->data[i].y) = 
				CV_MAT_ELEM(*param->imgMask, uchar, sample->x, sample->y);
		}
	}
	void expansion(CvMat* img, CvMat* imgMask, int cDif, int dDif) {
		long startTime = clock();
		if (verbose)
			cout << "in expansion" << endl;
		CvPointList* unknown = analyseTrimap(imgMask, NULL, NULL);
		if (verbose)
			cout << "unknown point: " << unknown->length << endl;
		CvMat* tmp;
		tmp = cvCloneMat(imgMask);

		double a, b = 0, step = unknown->length / double(pn) + 0.001;
		ExpansionPara *param;
		param = new ExpansionPara[pn];
		HANDLE *ht;
		ht = new HANDLE[pn];
		for (int i=0; i<pn; ++i) {
			a = b;
			b += step;
			param[i].img = img;
			param[i].imgMask = imgMask;
			param[i].cDif = cDif;
			param[i].dDif = dDif;
			param[i].tmp = tmp;
			param[i].unknown = unknown;
			param[i].start = int(a);
			param[i].end = int(b);
			ht[i] = CreateThread(NULL, 0, (LPTHREAD_START_ROUTINE)expansionK, &param[i], 0, NULL);
		}

		WaitForMultipleObjects(pn, ht, TRUE, 60000);
		for (int i=0; i<pn; ++i)
			CloseHandle(ht[i]);
		delete[] param;
		delete[] ht;

		delete[] unknown->data;
		delete unknown;
		cvReleaseMat(&tmp);

		//can delete
		if (verbose) {
			unknown = analyseTrimap(imgMask, NULL, NULL);
			cout << "after expansion unknown point: " << unknown->length << endl;
			delete[] unknown->data;
			delete unknown;
			long endTime = clock();
			cout << "expansed time: " << double(endTime - startTime) / CLOCKS_PER_SEC << endl;
		}
	}

	Mat CalAlpha(void) {
		for (int i=0; i<2; ++i) {
			expansion(img, imgMaskGray, 5-i/2, 10);
		}
		unknownList = analyseTrimap(imgMaskGray, NULL, NULL);
		un2PosMap = new int*[imgMaskGray->rows];
		for (int i=0; i<imgMaskGray->rows; ++i) {
			un2PosMap[i] = new int[imgMaskGray->cols];
			for (int j=0; j<imgMaskGray->cols; ++j)
				un2PosMap[i][j] = -1;
		}
#ifdef SHOW_WINDOW
		cvNamedWindow("expansed", CV_WINDOW_AUTOSIZE);
		cvShowImage("expansed", imgMaskGray);
#endif
		//	cvSaveMatImage("expansed.png", imgMaskGray);
		// 	cvSetMouseCallback("expansed", debugByMouseCh, imgMaskGray);
		// 	cvWaitKey(0);


		CvMat* unknownPart = cvCloneMat(imgMaskGray);
		CvMat* knownPart = cvCloneMat(imgMaskGray);
		for (int rowStart=0; rowStart<SPLIT+1; ++rowStart)
			for (int colStart=0; colStart<SPLIT+1; ++colStart) {
				int idx = (rowStart<<SPLIT) + colStart;
				fgPoint[idx].data = new CvPoint[img->rows*img->cols - unknownList->length];
				fgPoint[idx].length = 0;
				bgPoint[idx].data = new CvPoint[img->rows*img->cols - unknownList->length];
				bgPoint[idx].length = 0;
				CvMat *halfImgMask;
				if (SPLIT == 0)
					halfImgMask = cvCloneMat(imgMaskGray);
				else
					halfImgMask = pyrDownMat(imgMaskGray, rowStart, colStart);
				for (int i=0; i<halfImgMask->rows; ++i)
					for (int j=0; j<halfImgMask->cols; ++j) {
						uchar mv = CV_MAT_ELEM(*halfImgMask, uchar, i, j);
						if (mv == 0) {
							bgPoint[idx].data[bgPoint[idx].length].x = i;
							bgPoint[idx].data[bgPoint[idx].length].y = j;
							++bgPoint[idx].length;
						}
						if (mv == 255) {
							fgPoint[idx].data[fgPoint[idx].length].x = i;
							fgPoint[idx].data[fgPoint[idx].length].y = j;
							++fgPoint[idx].length;
						}
					}

					cvReleaseMat(&halfImgMask);
			}



			for (int i=0; i<imgMaskGray->rows; ++i)
				for (int j=0; j<imgMaskGray->cols; ++j) {
					uchar mv = CV_MAT_ELEM(*imgMaskGray, uchar, i, j);
					*CV_MAT_ELEM_PTR(*unknownPart, i, j) = (mv == 127 ? 1 : 0);
					*CV_MAT_ELEM_PTR(*knownPart, i, j) = (mv == 127 ? 0 : 1);
				}

				// 	CvMat* unknownImg;
				// 	unknownImg = cvCloneMat(img);
				// 	CvMat* knownImg;
				// 	knownImg = cvCloneMat(img);
				// 	CvMat* debug3;
				// 	debug3 = cvMul2(unknownPart, img);
				// 	cvConvert(debug3, unknownImg);
				// 	cvNamedWindow("unknownImg");
				// 	cvShowImage("unknownImg", unknownImg);
				// 	debug3 = cvMul2(knownPart, img);
				// 	cvConvert(debug3, knownImg);
				// 	cvNamedWindow("knownImg");
				// 	cvShowImage("knownImg", knownImg);
				// 	cvSaveMatImage("known.png", knownImg);


				unsampled.data = new CvPoint[10000];
				unsampled.length = 0;

				CvMat *selectf, *selectb, *sigmaf, *sigmab;
				selectb = selectf = sigmab = sigmaf = 0;
				sampleSelection(img, imgMaskGray, 4, selectf, selectb, sigmaf, sigmab);


				// 	CvMat *fg, *bg;
				// 	fg = cvCloneMat(img);
				// 	bg = cvCloneMat(img);
				// 	cvZero(fg);
				// 	cvZero(bg);
				// 	for (int i=0; i<unknownList->length; ++i) {
				// 		int a, b;
				// 		a = unknownList->data[i].x;
				// 		b = unknownList->data[i].y;
				// 		cvSet2D(fg, a, b, cvGet1D(selectf, i));
				// 		cvSet2D(bg, a, b, cvGet1D(selectb, i));
				// 	}
				// 
				// 	cvNamedWindow("fg");
				// 	cvShowImage("fg", fg);
				// 	cvNamedWindow("bg");
				// 	cvShowImage("bg", bg);
				// 
				// 	cvSaveMatImage("fg.png", fg);
				// 	cvSaveMatImage("bg.png", bg);
				// 	CvMat* unSampledMask;
				// 	unSampledMask = cvCloneMat(imgMaskGray);
				// 	cvZero(unSampledMask);
				// 	for (int i=0; i<unsampled.length; ++i)
				// 		cvSet2D(unSampledMask, unsampled.data[i].x, unsampled.data[i].y, cvScalar(255));
				// 	cvNamedWindow("unSampled");
				// 	cvShowImage("unSampled", unSampledMask);
				// 	cvSetMouseCallback("unSampled", debugByMouseCh, unSampledMask);

				// 	cvWaitKey(0);
				// 	return 0;


				CvMat *reff, *refb, *refAlpha, *conf;
				reff = refb = refAlpha = conf = 0;
				sampleRefinement(selectf, selectb, sigmaf, sigmab, img, imgMaskGray, 200, reff, refb, refAlpha, conf);


				// 	CvMat *fg2, *bg2;
				// 	fg2 = cvCloneMat(img);
				// 	bg2 = cvCloneMat(img);
				// 	cvZero(fg2);
				// 	cvZero(bg2);
				// 	for (int i=0; i<unknownList->length; ++i) {
				// 		int a, b;
				// 		a = unknownList->data[i].x;
				// 		b = unknownList->data[i].y;
				// 		cvSet2D(fg2, a, b, cvGet1D(reff, i));
				// 		cvSet2D(bg2, a, b, cvGet1D(refb, i));
				// 	}
				// 
				// 	cvNamedWindow("fg2");
				// 	cvShowImage("fg2", fg2);
				// 	cvNamedWindow("bg2");
				// 	cvShowImage("bg2", bg2);
				//     cvSaveMatImage("fg2.png", fg2);
				//     cvSaveMatImage("bg2.png", bg2);
				//	cvSetMouseCallback("fg2", debugByMouseCh, fg2);
				// 	cvSetMouseCallback("unknownImg", debugByMouseCh, unknownImg);
				// #ifdef _DEBUG
				// 	cvWaitKey(0);
				// 	return 0;
				// #endif
#ifdef SHOW_WINDOW
				cvNamedWindow("refine", CV_WINDOW_AUTOSIZE);
#endif
				CvMat *bMat, *fMat, *alphaMat, *confMat;
				bMat = fMat = alphaMat = confMat = 0;
				alphaMat = calWholeMat(reff, refb, refAlpha, conf, img, imgMaskGray, fMat, bMat, confMat);
#ifdef SHOW_WINDOW	
				cvShowImage("refine", alphaMat);
				cvSetMouseCallback("refine", debugByMouseDb, alphaMat);
#endif
				//	cvSaveMatImage("refine.png", alphaMat);


				CvMat* finalAlpha;
				finalAlpha = localSmoothing(fMat, bMat, alphaMat, confMat, img, imgMaskGray, 121);
#ifdef SHOW_WINDOW
				cvNamedWindow("smoothing", CV_WINDOW_AUTOSIZE);
				cvShowImage("smoothing", finalAlpha);
				//cvSetMouseCallback("smoothing", debugByMouseDb, finalAlpha);
#endif

				cvReleaseMat(&unknownPart);
				cvReleaseMat(&knownPart);
				cvReleaseMat(&selectf);
				cvReleaseMat(&selectb);
				cvReleaseMat(&sigmaf);
				cvReleaseMat(&sigmab);
				// 	cvReleaseMat(&fg);
				// 	cvReleaseMat(&bg);
				cvReleaseMat(&reff);
				cvReleaseMat(&refb);
				cvReleaseMat(&refAlpha);
				cvReleaseMat(&conf);
				// 	cvReleaseMat(&fg2);
				// 	cvReleaseMat(&bg2);
				cvReleaseMat(&bMat);
				cvReleaseMat(&fMat);
				cvReleaseMat(&alphaMat);
				cvReleaseMat(&confMat);

				return finalAlpha;
	}

public:
	int SharedMatting(Mat& img, Mat& mask)
	{
		Mat refinedMask;
		if (mask.channels() == 3)
			refinedMask = RefineMask(RGB2Gray(img));
		else
			refinedMask = RefineMask(img);

		vector<Point> fg, bg, unknown;
		AnalyseTrimap(refinedMask, fg, bg, unknown);
		unknownPixels = unknown.size();

		//these sentences decide we can't compile the code to DLL !!!
		Mat finalAlpha = CalAlpha();
		cvReleaseMat(&imgMaskGray);
		return finalAlpha;

		return 0;
	}

};

CvMat* sharedMatting(CvMat *inImg, CvMat *inMaskImg, bool v) {
	verbose = v;
	
	if (CV_MAT_CN(inMaskImg->type) == 3)
		imgMaskGray = RefineMask(rgb2gray(inMaskImg));
	else
		imgMaskGray = RefineMask(inMaskImg);

	CvPointList *tmpPl = analyseTrimap(imgMaskGray, NULL, NULL);
	unknownPixels = tmpPl->length;
	delete[] tmpPl->data;
	delete tmpPl;

	//these sentences decide we can't compile the code to DLL !!!
	CvMat *finalAlpha = calAlpha();
	cvReleaseMat(&imgMaskGray);
	return finalAlpha;
}

CvMat* sharedMatting(char imgName[], char maskName[], bool v) {
	img = imread(imgName);
	imgMask = imread(maskName);
	CvMat *finalAlpha = sharedMatting(img, imgMask, v);
	cvReleaseMat(&img);
	cvReleaseMat(&imgMask);
	return finalAlpha;
}